1 /* -*- c -*- */ 2 /* 3 * Copyright 2007 - 2013 Dominic Spill, Michael Ossmann, Will Code 4 * 5 * This file is part of libbtbb 6 * 7 * This program is free software; you can redistribute it and/or modify 8 * it under the terms of the GNU General Public License as published by 9 * the Free Software Foundation; either version 2, or (at your option) 10 * any later version. 11 * 12 * This program is distributed in the hope that it will be useful, 13 * but WITHOUT ANY WARRANTY; without even the implied warranty of 14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 * GNU General Public License for more details. 16 * 17 * You should have received a copy of the GNU General Public License 18 * along with libbtbb; see the file COPYING. If not, write to 19 * the Free Software Foundation, Inc., 51 Franklin Street, 20 * Boston, MA 02110-1301, USA. 21 */ 22 23 #include <stdio.h> 24 #include <stdlib.h> 25 26 #include "bluetooth_packet.h" 27 #include "uthash.h" 28 #include "sw_check_tables.h" 29 #include "version.h" 30 31 #define MIN(X, Y) (((X) < (Y)) ? (X) : (Y)) 32 33 /* Maximum number of AC errors supported by library. Caller may 34 * specify any value <= AC_ERROR_LIMIT in btbb_init(). */ 35 #define AC_ERROR_LIMIT 5 36 37 /* maximum number of bit errors for known syncwords */ 38 #define MAX_SYNCWORD_ERRS 5 39 40 /* maximum number of bit errors in */ 41 #define MAX_BARKER_ERRORS 1 42 43 /* default codeword modified for PN sequence and barker code */ 44 #define DEFAULT_CODEWORD 0xb0000002c7820e7eULL 45 46 /* Default access code, used for calculating syndromes */ 47 #define DEFAULT_AC 0xcc7b7268ff614e1bULL 48 49 /* index into whitening data array */ 50 static const uint8_t INDICES[] = {99, 85, 17, 50, 102, 58, 108, 45, 92, 62, 32, 118, 88, 11, 80, 2, 37, 69, 55, 8, 20, 40, 74, 114, 15, 106, 30, 78, 53, 72, 28, 26, 68, 7, 39, 113, 105, 77, 71, 25, 84, 49, 57, 44, 61, 117, 10, 1, 123, 124, 22, 125, 111, 23, 42, 126, 6, 112, 76, 24, 48, 43, 116, 0}; 51 52 /* whitening data */ 53 static const uint8_t WHITENING_DATA[] = {1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 1, 1, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, 1, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1}; 54 55 /* lookup table for barker code hamming distance */ 56 static const uint8_t BARKER_DISTANCE[] = { 57 3,3,3,2,3,2,2,1,2,3,3,3,3,3,3,2,2,3,3,3,3,3,3,2,1,2,2,3,2,3,3,3, 58 3,2,2,1,2,1,1,0,3,3,3,2,3,2,2,1,3,3,3,2,3,2,2,1,2,3,3,3,3,3,3,2, 59 2,3,3,3,3,3,3,2,1,2,2,3,2,3,3,3,1,2,2,3,2,3,3,3,0,1,1,2,1,2,2,3, 60 3,3,3,2,3,2,2,1,2,3,3,3,3,3,3,2,2,3,3,3,3,3,3,2,1,2,2,3,2,3,3,3}; 61 62 /* string representations of packet type */ 63 static const char * const TYPE_NAMES[] = { 64 "NULL", "POLL", "FHS", "DM1", "DH1/2-DH1", "HV1", "HV2/2-EV3", "HV3/EV3/3-EV3", 65 "DV/3-DH1", "AUX1", "DM3/2-DH3", "DH3/3-DH3", "EV4/2-EV5", "EV5/3-EV5", "DM5/2-DH5", "DH5/3-DH5" 66 }; 67 68 /* 69 * generator matrix for sync word (64,30) linear block code 70 * based on polynomial 0260534236651 71 * thanks to http://www.ee.unb.ca/cgi-bin/tervo/polygen.pl 72 * modified for barker code 73 */ 74 static const uint64_t sw_matrix[] = { 75 0xfe000002a0d1c014ULL, 0x01000003f0b9201fULL, 0x008000033ae40edbULL, 0x004000035fca99b9ULL, 76 0x002000036d5dd208ULL, 0x00100001b6aee904ULL, 0x00080000db577482ULL, 0x000400006dabba41ULL, 77 0x00020002f46d43f4ULL, 0x000100017a36a1faULL, 0x00008000bd1b50fdULL, 0x000040029c3536aaULL, 78 0x000020014e1a9b55ULL, 0x0000100265b5d37eULL, 0x0000080132dae9bfULL, 0x000004025bd5ea0bULL, 79 0x00000203ef526bd1ULL, 0x000001033511ab3cULL, 0x000000819a88d59eULL, 0x00000040cd446acfULL, 80 0x00000022a41aabb3ULL, 0x0000001390b5cb0dULL, 0x0000000b0ae27b52ULL, 0x0000000585713da9ULL}; 81 82 static const uint64_t syndrome_matrix[] = { 83 0x4be2573a00000000ULL, 0x25f12b9d00000000ULL, 0x591ac2f480000000ULL, 0x676f364040000000ULL, 84 0x33b79b2020000000ULL, 0x19dbcd9010000000ULL, 0x0cede6c808000000ULL, 0x0676f36404000000ULL, 85 0x48d92e8802000000ULL, 0x246c974401000000ULL, 0x59d41c9800800000ULL, 0x2cea0e4c00400000ULL, 86 0x5d97501c00200000ULL, 0x6529ff3400100000ULL, 0x7976a8a000080000ULL, 0x3cbb545000040000ULL, 87 0x1e5daa2800020000ULL, 0x0f2ed51400010000ULL, 0x4c753db000008000ULL, 0x263a9ed800004000ULL, 88 0x131d4f6c00002000ULL, 0x426cf08c00001000ULL, 0x6ad42f7c00000800ULL, 0x7e88408400000400ULL, 89 0x74a6777800000200ULL, 0x3a533bbc00000100ULL, 0x56cbcae400000080ULL, 0x6087b24800000040ULL, 90 0x3043d92400000020ULL, 0x53c3bba800000010ULL, 0x29e1ddd400000008ULL, 0x5f12b9d000000004ULL, 91 0x2f895ce800000002ULL, 0x97c4ae7400000001ULL}; 92 93 static const uint64_t barker_correct[] = { 94 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 95 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 96 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 97 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 98 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 99 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 100 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 101 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 102 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 103 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 104 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 105 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 106 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 107 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 108 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 109 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 110 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 111 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 112 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 113 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 114 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 115 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 116 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 117 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 118 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 119 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 120 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 121 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 122 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 123 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 124 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 125 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL}; 126 127 static const uint64_t pn = 0x83848D96BBCC54FCULL; 128 129 static const uint16_t fec23_gen_matrix[] = { 130 0x2c01, 0x5802, 0x1c04, 0x3808, 0x7010, 131 0x4c20, 0x3440, 0x6880, 0x7d00, 0x5600}; 132 133 typedef struct { 134 uint64_t syndrome; /* key */ 135 uint64_t error; 136 UT_hash_handle hh; 137 } syndrome_struct; 138 139 static syndrome_struct *syndrome_map = NULL; 140 141 static void add_syndrome(uint64_t syndrome, uint64_t error) 142 { 143 syndrome_struct *s; 144 s = malloc(sizeof(syndrome_struct)); 145 s->syndrome = syndrome; 146 s->error = error; 147 148 HASH_ADD(hh, syndrome_map, syndrome, 8, s); 149 } 150 151 static syndrome_struct *find_syndrome(uint64_t syndrome) 152 { 153 syndrome_struct *s; 154 155 HASH_FIND(hh, syndrome_map, &syndrome, 8, s); 156 return s; 157 } 158 159 static uint64_t gen_syndrome(uint64_t codeword) 160 { 161 uint64_t syndrome = codeword & 0xffffffff; 162 codeword >>= 32; 163 syndrome ^= sw_check_table4[codeword & 0xff]; 164 codeword >>= 8; 165 syndrome ^= sw_check_table5[codeword & 0xff]; 166 codeword >>= 8; 167 syndrome ^= sw_check_table6[codeword & 0xff]; 168 codeword >>= 8; 169 syndrome ^= sw_check_table7[codeword & 0xff]; 170 return syndrome; 171 } 172 173 static void cycle(uint64_t error, int start, int depth, uint64_t codeword) 174 { 175 uint64_t new_error, syndrome, base; 176 int i; 177 base = 1; 178 depth -= 1; 179 for (i = start; i < 58; i++) 180 { 181 new_error = (base << i); 182 new_error |= error; 183 if (depth) 184 cycle(new_error, i + 1, depth, codeword); 185 else { 186 syndrome = gen_syndrome(codeword ^ new_error); 187 add_syndrome(syndrome, new_error); 188 } 189 } 190 } 191 192 static void gen_syndrome_map(int bit_errors) 193 { 194 int i; 195 for(i = 1; i <= bit_errors; i++) 196 cycle(0, 0, i, DEFAULT_AC); 197 } 198 199 /* Generate Sync Word from an LAP */ 200 uint64_t btbb_gen_syncword(const int LAP) 201 { 202 int i; 203 uint64_t codeword = DEFAULT_CODEWORD; 204 205 /* the sync word generated is in host order, not air order */ 206 for (i = 0; i < 24; i++) 207 if (LAP & (0x800000 >> i)) 208 codeword ^= sw_matrix[i]; 209 210 return codeword; 211 } 212 213 static void init_packet(btbb_packet *pkt, uint32_t lap, uint8_t ac_errors) 214 { 215 pkt->LAP = lap; 216 pkt->ac_errors = ac_errors; 217 218 pkt->flags = 0; 219 btbb_packet_set_flag(pkt, BTBB_WHITENED, 1); 220 } 221 222 /* Convert some number of bits of an air order array to a host order integer */ 223 static uint8_t air_to_host8(const char *air_order, const int bits) 224 { 225 int i; 226 uint8_t host_order = 0; 227 for (i = 0; i < bits; i++) 228 host_order |= ((uint8_t)air_order[i] << i); 229 return host_order; 230 } 231 static uint16_t air_to_host16(const char *air_order, const int bits) 232 { 233 int i; 234 uint16_t host_order = 0; 235 for (i = 0; i < bits; i++) 236 host_order |= ((uint16_t)air_order[i] << i); 237 return host_order; 238 } 239 static uint32_t air_to_host32(const char *air_order, const int bits) 240 { 241 int i; 242 uint32_t host_order = 0; 243 for (i = 0; i < bits; i++) 244 host_order |= ((uint32_t)air_order[i] << i); 245 return host_order; 246 } 247 static uint64_t air_to_host64(const char *air_order, const int bits) 248 { 249 int i; 250 uint64_t host_order = 0; 251 for (i = 0; i < bits; i++) 252 host_order |= ((uint64_t)air_order[i] << i); 253 return host_order; 254 } 255 256 /* Convert some number of bits in a host order integer to an air order array */ 257 static void host_to_air(const uint8_t host_order, char *air_order, const int bits) 258 { 259 int i; 260 for (i = 0; i < bits; i++) 261 air_order[i] = (host_order >> i) & 0x01; 262 } 263 /* count the number of 1 bits in a uint64_t */ 264 static uint8_t count_bits(uint64_t n) 265 { 266 #ifdef __GNUC__ 267 return (uint8_t) __builtin_popcountll (n); 268 #else 269 uint8_t i = 0; 270 for (i = 0; n != 0; i++) 271 n &= n - 1; 272 return i; 273 #endif 274 } 275 276 #ifndef RELEASE 277 #define RELEASE "unknown" 278 #endif 279 char *btbb_get_release(void) { 280 return RELEASE; 281 } 282 283 #ifndef VERSION 284 #define VERSION "unknown" 285 #endif 286 char *btbb_get_version(void) { 287 return VERSION; 288 } 289 290 int btbb_init(int max_ac_errors) 291 { 292 /* Sanity check max_ac_errors. */ 293 if ( (max_ac_errors < 0) || (max_ac_errors > AC_ERROR_LIMIT) ) { 294 fprintf(stderr, "%s: max_ac_errors out of range\n", 295 __FUNCTION__); 296 return -1; 297 } 298 299 if ((syndrome_map == NULL) && (max_ac_errors)) 300 gen_syndrome_map(max_ac_errors); 301 302 return 0; 303 } 304 305 btbb_packet * 306 btbb_packet_new(void) 307 { 308 btbb_packet *pkt = (btbb_packet *)calloc(1, sizeof(btbb_packet)); 309 if(pkt) 310 pkt->refcount = 1; 311 else 312 fprintf(stderr, "Unable to allocate packet"); 313 return pkt; 314 } 315 316 void 317 btbb_packet_ref(btbb_packet *pkt) 318 { 319 pkt->refcount++; 320 } 321 322 void 323 btbb_packet_unref(btbb_packet *pkt) 324 { 325 pkt->refcount--; 326 if (pkt->refcount == 0) 327 free(pkt); 328 } 329 330 uint32_t btbb_packet_get_lap(const btbb_packet *pkt) 331 { 332 return pkt->LAP; 333 } 334 335 void btbb_packet_set_uap(btbb_packet *pkt, uint8_t uap) 336 { 337 pkt->UAP = uap; 338 btbb_packet_set_flag(pkt, BTBB_UAP_VALID, 1); 339 } 340 341 uint8_t btbb_packet_get_uap(const btbb_packet *pkt) 342 { 343 return pkt->UAP; 344 } 345 346 uint16_t btbb_packet_get_nap(const btbb_packet *pkt) 347 { 348 return pkt->NAP; 349 } 350 351 uint32_t btbb_packet_get_clkn(const btbb_packet *pkt) { 352 return pkt->clkn; 353 } 354 355 uint8_t btbb_packet_get_channel(const btbb_packet *pkt) { 356 return pkt->channel; 357 } 358 359 void btbb_packet_set_modulation(btbb_packet *pkt, uint8_t modulation) { 360 pkt->modulation = modulation; 361 } 362 363 uint8_t btbb_packet_get_modulation(const btbb_packet *pkt) { 364 return pkt->modulation; 365 } 366 367 void btbb_packet_set_transport(btbb_packet *pkt, uint8_t transport) { 368 pkt->transport = transport; 369 } 370 371 uint8_t btbb_packet_get_transport(const btbb_packet *pkt) { 372 return pkt->transport; 373 } 374 375 uint8_t btbb_packet_get_ac_errors(const btbb_packet *pkt) { 376 return pkt->ac_errors; 377 } 378 379 int promiscuous_packet_search(char *stream, int search_length, uint32_t *lap, int max_ac_errors, uint8_t *ac_errors) { 380 uint64_t syncword, codeword, syndrome, corrected_barker, ac; 381 syndrome_struct *errors; 382 char *symbols; 383 int count, offset = -1; 384 385 /* Barker code at end of sync word (includes 386 * MSB of LAP) is used as a rough filter. 387 */ 388 uint8_t barker = air_to_host8(&stream[57], 6); 389 barker <<= 1; 390 391 for (count = 0; count < search_length; count++) { 392 symbols = &stream[count]; 393 barker >>= 1; 394 barker |= (symbols[63] << 6); 395 if (BARKER_DISTANCE[barker] <= MAX_BARKER_ERRORS) { 396 // Error correction 397 syncword = air_to_host64(symbols, 64); 398 399 /* correct the barker code with a simple comparison */ 400 corrected_barker = barker_correct[(uint8_t)(syncword >> 57)]; 401 syncword = (syncword & 0x01ffffffffffffffULL) | corrected_barker; 402 403 codeword = syncword ^ pn; 404 405 /* Zero syndrome -> good codeword. */ 406 syndrome = gen_syndrome(codeword); 407 *ac_errors = 0; 408 409 /* Try to fix errors in bad codeword. */ 410 if (syndrome) { 411 errors = find_syndrome(syndrome); 412 if (errors != NULL) { 413 syncword ^= errors->error; 414 *ac_errors = count_bits(errors->error); 415 syndrome = 0; 416 } 417 else { 418 *ac_errors = 0xff; // fail 419 } 420 } 421 422 if (*ac_errors <= max_ac_errors) { 423 *lap = (syncword >> 34) & 0xffffff; 424 offset = count; 425 break; 426 } 427 } 428 } 429 return offset; 430 } 431 432 /* Matching a specific LAP */ 433 int find_known_lap(char *stream, int search_length, uint32_t lap, int max_ac_errors, uint8_t *ac_errors) { 434 uint64_t syncword, codeword, syndrome, corrected_barker, ac; 435 syndrome_struct *errors; 436 char *symbols; 437 int count, offset = -1; 438 439 ac = btbb_gen_syncword(lap); 440 for (count = 0; count < search_length; count++) { 441 symbols = &stream[count]; 442 syncword = air_to_host64(symbols, 64); 443 *ac_errors = count_bits(syncword ^ ac); 444 445 if (*ac_errors <= max_ac_errors) { 446 offset = count; 447 //printf("Offset = %d\n", offset); 448 break; 449 } 450 } 451 return offset; 452 } 453 454 /* Looks for an AC in the stream */ 455 int btbb_find_ac(char *stream, int search_length, uint32_t lap, int max_ac_errors, btbb_packet **pkt_ptr) { 456 int offset; 457 uint8_t ac_errors; 458 459 /* Matching any LAP */ 460 if (lap == LAP_ANY) 461 offset = promiscuous_packet_search(stream, search_length, &lap, 462 max_ac_errors, &ac_errors); 463 else 464 offset = find_known_lap(stream, search_length, lap, 465 max_ac_errors, &ac_errors); 466 467 if (offset >= 0) { 468 if (*pkt_ptr == NULL) 469 *pkt_ptr = btbb_packet_new(); 470 init_packet(*pkt_ptr, lap, ac_errors); 471 } 472 473 return offset; 474 } 475 476 /* Copy data (symbols) into packet and set rx data. */ 477 void btbb_packet_set_data(btbb_packet *pkt, char *data, int length, uint8_t channel, uint32_t clkn) 478 { 479 int i; 480 481 if (length > MAX_SYMBOLS) 482 length = MAX_SYMBOLS; 483 for (i = 0; i < length; i++) 484 pkt->symbols[i] = data[i]; 485 486 pkt->length = length; 487 pkt->channel = channel; 488 pkt->clkn = clkn >> 1; // really CLK1 489 } 490 491 void btbb_packet_set_flag(btbb_packet *pkt, int flag, int val) 492 { 493 uint32_t mask = 1L << flag; 494 pkt->flags &= ~mask; 495 if (val) 496 pkt->flags |= mask; 497 } 498 499 int btbb_packet_get_flag(const btbb_packet *pkt, int flag) 500 { 501 uint32_t mask = 1L << flag; 502 return ((pkt->flags & mask) != 0); 503 } 504 505 const char *btbb_get_symbols(const btbb_packet* pkt) 506 { 507 return (const char*) pkt->symbols; 508 } 509 510 int btbb_packet_get_payload_length(const btbb_packet* pkt) 511 { 512 return pkt->payload_length; 513 } 514 515 const char *btbb_get_payload(const btbb_packet* pkt) 516 { 517 return (const char*) pkt->payload; 518 } 519 520 int btbb_get_payload_packed(const btbb_packet* pkt, char *dst) 521 { 522 int i; 523 for(i=0;i<pkt->payload_length;i++) 524 dst[i] = (char) air_to_host8(&pkt->payload[i*8], 8); 525 return pkt->payload_length; 526 } 527 528 uint8_t btbb_packet_get_type(const btbb_packet* pkt) 529 { 530 return pkt->packet_type; 531 } 532 533 uint8_t btbb_packet_get_lt_addr(const btbb_packet* pkt) 534 { 535 return pkt->packet_lt_addr; 536 } 537 538 uint8_t btbb_packet_get_header_flags(const btbb_packet* pkt) 539 { 540 return pkt->packet_flags; 541 } 542 543 uint8_t btbb_packet_get_hec(const btbb_packet* pkt) 544 { 545 return pkt->packet_hec; 546 } 547 548 uint32_t btbb_packet_get_header_packed(const btbb_packet* pkt) 549 { 550 return air_to_host32(&pkt->packet_header[0], 18); 551 } 552 553 /* Compare stream with sync word 554 * Unused, but useful to correct >3 bit errors with known LAP 555 */ 556 static int check_syncword(uint64_t streamword, uint64_t syncword) 557 { 558 uint8_t biterrors; 559 560 //FIXME do error correction instead of detection 561 biterrors = count_bits(streamword ^ syncword); 562 563 if (biterrors >= 5) 564 return 0; 565 566 return 1; 567 } 568 569 /* Reverse the bits in a byte */ 570 static uint8_t reverse(char byte) 571 { 572 return (byte & 0x80) >> 7 | (byte & 0x40) >> 5 | (byte & 0x20) >> 3 | (byte & 0x10) >> 1 | (byte & 0x08) << 1 | (byte & 0x04) << 3 | (byte & 0x02) << 5 | (byte & 0x01) << 7; 573 } 574 575 576 /* Decode 1/3 rate FEC, three like symbols in a row */ 577 static int unfec13(char *input, char *output, int length) 578 { 579 int a, b, c, i; 580 int be = 0; /* bit errors */ 581 582 for (i = 0; i < length; i++) { 583 a = 3 * i; 584 b = a + 1; 585 c = a + 2; 586 output[i] = ((input[a] & input[b]) | (input[b] & input[c]) | 587 (input[c] & input[a])); 588 be += ((input[a] ^ input[b]) | (input[b] ^ input[c]) | 589 (input[c] ^ input[a])); 590 } 591 592 return (be < (length / 4)); 593 } 594 595 /* encode 10 bits with 2/3 rate FEC code, a (15,10) shortened Hamming code */ 596 static uint16_t fec23(uint16_t data) 597 { 598 int i; 599 uint16_t codeword = 0; 600 601 /* host order, not air order */ 602 for (i = 0; i < 10; i++) 603 if (data & (1 << i)) 604 codeword ^= fec23_gen_matrix[i]; 605 606 return codeword; 607 } 608 609 /* Decode 2/3 rate FEC, a (15,10) shortened Hamming code */ 610 static char *unfec23(char *input, int length) 611 { 612 /* input points to the input data 613 * length is length in bits of the data 614 * before it was encoded with fec2/3 */ 615 int iptr, optr, count; 616 char* output; 617 uint8_t diff, check; 618 uint16_t data, codeword; 619 620 diff = length % 10; 621 // padding at end of data 622 if(0!=diff) 623 length += (10 - diff); 624 625 output = (char *) malloc(length); 626 627 for (iptr = 0, optr = 0; optr<length; iptr += 15, optr += 10) { 628 // copy data to output 629 for(count=0;count<10;count++) 630 output[optr+count] = input[iptr+count]; 631 632 // grab data and error check in host format 633 data = air_to_host16(input+iptr, 10); 634 check = air_to_host8(input+iptr+10, 5); 635 636 // call fec23 on data to generate the codeword 637 codeword = fec23(data); 638 diff = check ^ (codeword >> 10); 639 640 /* no errors or single bit errors (errors in the parity bit): 641 * (a strong hint it's a real packet) 642 * Otherwise we need to corret the output*/ 643 if (diff & (diff - 1)) { 644 switch (diff) { 645 /* comments are the bit that's wrong and the value 646 * of diff in air order, from the BT spec */ 647 // 1000000000 11010 648 case 0x0b: output[optr] ^= 1; break; 649 // 0100000000 01101 650 case 0x16: output[optr+1] ^= 1; break; 651 // 0010000000 11100 652 case 0x07: output[optr+2] ^= 1; break; 653 // 0001000000 01110 654 case 0x0e: output[optr+3] ^= 1; break; 655 // 0000100000 00111 656 case 0x1c: output[optr+4] ^= 1; break; 657 // 0000010000 11001 658 case 0x13: output[optr+5] ^= 1; break; 659 // 0000001000 10110 660 case 0x0d: output[optr+6] ^= 1; break; 661 // 0000000100 01011 662 case 0x1a: output[optr+7] ^= 1; break; 663 // 0000000010 11111 664 case 0x1f: output[optr+8] ^= 1; break; 665 // 0000000001 10101 666 case 0x15: output[optr+9] ^= 1; break; 667 /* not one of these errors, probably multiple bit errors 668 * or maybe not a real packet, safe to drop it? */ 669 default: free(output); return 0; 670 } 671 } 672 } 673 return output; 674 } 675 676 677 /* Remove the whitening from an air order array */ 678 static void unwhiten(char* input, char* output, int clock, int length, int skip, btbb_packet* pkt) 679 { 680 int count, index; 681 index = INDICES[clock & 0x3f]; 682 index += skip; 683 index %= 127; 684 685 for(count = 0; count < length; count++) 686 { 687 /* unwhiten if whitened, otherwise just copy input to output */ 688 output[count] = btbb_packet_get_flag(pkt, BTBB_WHITENED) ? 689 input[count] ^ WHITENING_DATA[index] : input[count]; 690 index += 1; 691 index %= 127; 692 } 693 } 694 695 /* Pointer to start of packet, length of packet in bits, UAP */ 696 static uint16_t crcgen(char *payload, int length, int UAP) 697 { 698 char bit; 699 uint16_t reg, count; 700 701 reg = (reverse(UAP) << 8) & 0xff00; 702 for(count = 0; count < length; count++) 703 { 704 bit = payload[count]; 705 706 reg = (reg >> 1) | (((reg & 0x0001) ^ (bit & 0x01))<<15); 707 708 /*Bit 5*/ 709 reg ^= ((reg & 0x8000)>>5); 710 711 /*Bit 12*/ 712 reg ^= ((reg & 0x8000)>>12); 713 } 714 return reg; 715 } 716 717 /* extract UAP by reversing the HEC computation */ 718 static uint8_t uap_from_hec(uint16_t data, uint8_t hec) 719 { 720 int i; 721 722 for (i = 9; i >= 0; i--) { 723 /* 0x65 is xor'd if MSB is 1, else 0x00 (which does nothing) */ 724 if (hec & 0x80) 725 hec ^= 0x65; 726 727 hec = (hec << 1) | (((hec >> 7) ^ (data >> i)) & 0x01); 728 } 729 return reverse(hec); 730 } 731 732 /* check if the packet's CRC is correct for a given clock (CLK1-6) */ 733 int crc_check(int clock, btbb_packet* pkt) 734 { 735 /* 736 * return value of 1 represents inconclusive result (default) 737 * return value > 1 represents positive result (e.g. CRC match) 738 * return value of 0 represents negative result (e.g. CRC failure without 739 * the possibility that we have assumed the wrong logical transport) 740 */ 741 int retval = 1; 742 743 switch(pkt->packet_type) 744 { 745 case PACKET_TYPE_FHS: 746 retval = fhs(clock, pkt); 747 break; 748 749 case PACKET_TYPE_DV: 750 case PACKET_TYPE_DM1: 751 case PACKET_TYPE_DM3: 752 case PACKET_TYPE_DM5: 753 retval = DM(clock, pkt); 754 break; 755 756 case PACKET_TYPE_DH1: 757 case PACKET_TYPE_DH3: 758 case PACKET_TYPE_DH5: 759 retval = DH(clock, pkt); 760 break; 761 762 case PACKET_TYPE_HV3: /* EV3 */ 763 retval = EV3(clock, pkt); 764 break; 765 case PACKET_TYPE_EV4: 766 retval = EV4(clock, pkt); 767 break; 768 case PACKET_TYPE_EV5: 769 retval = EV5(clock, pkt); 770 break; 771 772 case PACKET_TYPE_HV1: 773 retval = HV(clock, pkt); 774 break; 775 776 /* some types can't help us */ 777 default: 778 break; 779 } 780 /* 781 * never return a zero result unless this is a FHS, DM1, or HV1. any 782 * other type could have actually been something else (another logical 783 * transport) 784 */ 785 if (retval == 0 && (pkt->packet_type != 2 && pkt->packet_type != 3 && 786 pkt->packet_type != 5)) 787 return 1; 788 789 /* EV3 and EV5 have a relatively high false positive rate */ 790 if (retval > 1 && (pkt->packet_type == 7 || pkt->packet_type == 13)) 791 return 1; 792 793 return retval; 794 } 795 796 /* verify the payload CRC */ 797 static int payload_crc(btbb_packet* pkt) 798 { 799 uint16_t crc; /* CRC calculated from payload data */ 800 uint16_t check; /* CRC supplied by packet */ 801 802 crc = crcgen(pkt->payload, (pkt->payload_length - 2) * 8, pkt->UAP); 803 check = air_to_host16(&pkt->payload[(pkt->payload_length - 2) * 8], 16); 804 805 return (crc == check); 806 } 807 808 int fhs(int clock, btbb_packet* pkt) 809 { 810 /* skip the access code and packet header */ 811 char *stream = pkt->symbols + 122; 812 /* number of symbols remaining after access code and packet header */ 813 int size = pkt->length - 122; 814 815 pkt->payload_length = 20; 816 817 if (size < pkt->payload_length * 12) 818 return 1; //FIXME should throw exception 819 820 char *corrected = unfec23(stream, pkt->payload_length * 8); 821 if (!corrected) 822 return 0; 823 824 /* try to unwhiten with known clock bits */ 825 unwhiten(corrected, pkt->payload, clock, pkt->payload_length * 8, 18, pkt); 826 if (payload_crc(pkt)) { 827 free(corrected); 828 return 1000; 829 } 830 831 /* try all 32 possible X-input values instead */ 832 for (clock = 32; clock < 64; clock++) { 833 unwhiten(corrected, pkt->payload, clock, pkt->payload_length * 8, 18, pkt); 834 if (payload_crc(pkt)) { 835 free(corrected); 836 return 1000; 837 } 838 } 839 840 /* failed to unwhiten */ 841 free(corrected); 842 return 0; 843 } 844 845 /* decode payload header, return value indicates success */ 846 static int decode_payload_header(char *stream, int clock, int header_bytes, int size, int fec, btbb_packet* pkt) 847 { 848 if(header_bytes == 2) 849 { 850 if(size < 16) 851 return 0; //FIXME should throw exception 852 if(fec) { 853 if(size < 30) 854 return 0; //FIXME should throw exception 855 char *corrected = unfec23(stream, 16); 856 if (!corrected) 857 return 0; 858 unwhiten(corrected, pkt->payload_header, clock, 16, 18, pkt); 859 free(corrected); 860 } else { 861 unwhiten(stream, pkt->payload_header, clock, 16, 18, pkt); 862 } 863 /* payload length is payload body length + 2 bytes payload header + 2 bytes CRC */ 864 pkt->payload_length = air_to_host16(&pkt->payload_header[3], 10) + 4; 865 } else { 866 if(size < 8) 867 return 0; //FIXME should throw exception 868 if(fec) { 869 if(size < 15) 870 return 0; //FIXME should throw exception 871 char *corrected = unfec23(stream, 8); 872 if (!corrected) 873 return 0; 874 unwhiten(corrected, pkt->payload_header, clock, 8, 18, pkt); 875 free(corrected); 876 } else { 877 unwhiten(stream, pkt->payload_header, clock, 8, 18, pkt); 878 } 879 /* payload length is payload body length + 1 byte payload header + 2 bytes CRC */ 880 pkt->payload_length = air_to_host8(&pkt->payload_header[3], 5) + 3; 881 } 882 /* Try to set the max payload length to a sensible value, 883 * especially when using strange data 884 */ 885 int max_length = 0; 886 switch(pkt->packet_type) { 887 case PACKET_TYPE_DM1: 888 max_length = 20; 889 break; 890 case PACKET_TYPE_DH1: 891 /* assuming DH1 but could be 2-DH1 (58) */ 892 max_length = 30; 893 break; 894 case PACKET_TYPE_DV: 895 /* assuming DV but could be 3-DH1 (87) */ 896 max_length = 12; /* + 10bytes of voice data */ 897 break; 898 case PACKET_TYPE_DM3: 899 /* assuming DM3 but could be 2-DH3 (371) */ 900 max_length = 125; 901 break; 902 case PACKET_TYPE_DH3: 903 /* assuming DH3 but could be 3-DH3 (556) */ 904 max_length = 187; 905 break; 906 case PACKET_TYPE_DM5: 907 /* assuming DM5 but could be 2-DH5 (683) */ 908 max_length = 228; 909 break; 910 case PACKET_TYPE_DH5: 911 /* assuming DH5 but could be 3-DH5 (1025) */ 912 max_length = 343; 913 break; 914 } 915 pkt->payload_length = MIN(pkt->payload_length, max_length); 916 pkt->payload_llid = air_to_host8(&pkt->payload_header[0], 2); 917 pkt->payload_flow = air_to_host8(&pkt->payload_header[2], 1); 918 pkt->payload_header_length = header_bytes; 919 return 1; 920 } 921 922 /* DM 1/3/5 packet (and DV)*/ 923 int DM(int clock, btbb_packet* pkt) 924 { 925 int bitlength; 926 /* number of bytes in the payload header */ 927 int header_bytes = 2; 928 /* maximum payload length */ 929 int max_length; 930 /* skip the access code and packet header */ 931 char *stream = pkt->symbols + 122; 932 /* number of symbols remaining after access code and packet header */ 933 int size = pkt->length - 122; 934 935 switch(pkt->packet_type) 936 { 937 case PACKET_TYPE_DV: 938 /* skip 80 voice bits, then treat the rest like a DM1 */ 939 stream += 80; 940 size -= 80; 941 header_bytes = 1; 942 /* I don't think the length of the voice field ("synchronous data 943 * field") is included in the length indicated by the payload 944 * header in the data field ("asynchronous data field"), but I 945 * could be wrong. 946 */ 947 max_length = 12; 948 break; 949 case PACKET_TYPE_DM1: 950 header_bytes = 1; 951 max_length = 20; 952 break; 953 case PACKET_TYPE_DM3: 954 max_length = 125; 955 break; 956 case PACKET_TYPE_DM5: 957 max_length = 228; 958 break; 959 default: /* not a DM1/3/5 or DV */ 960 return 0; 961 } 962 if(!decode_payload_header(stream, clock, header_bytes, size, 1, pkt)) 963 return 0; 964 /* check that the length indicated in the payload header is within spec */ 965 if(pkt->payload_length > max_length) 966 /* could be encrypted */ 967 return 1; 968 bitlength = pkt->payload_length*8; 969 if(bitlength > size) 970 return 1; //FIXME should throw exception 971 972 char *corrected = unfec23(stream, bitlength); 973 if (!corrected) 974 return 0; 975 unwhiten(corrected, pkt->payload, clock, bitlength, 18, pkt); 976 free(corrected); 977 978 if (payload_crc(pkt)) 979 return 10; 980 981 /* could be encrypted */ 982 return 2; 983 } 984 985 /* DH 1/3/5 packet (and AUX1) */ 986 /* similar to DM 1/3/5 but without FEC */ 987 int DH(int clock, btbb_packet* pkt) 988 { 989 int bitlength; 990 /* number of bytes in the payload header */ 991 int header_bytes = 2; 992 /* maximum payload length */ 993 int max_length; 994 /* skip the access code and packet header */ 995 char *stream = pkt->symbols + 122; 996 /* number of symbols remaining after access code and packet header */ 997 int size = pkt->length - 122; 998 999 switch(pkt->packet_type) 1000 { 1001 case PACKET_TYPE_AUX1: 1002 case PACKET_TYPE_DH1: 1003 header_bytes = 1; 1004 max_length = 30; 1005 break; 1006 case PACKET_TYPE_DH3: 1007 max_length = 187; 1008 break; 1009 case PACKET_TYPE_DH5: 1010 max_length = 343; 1011 break; 1012 default: /* not a DH1/3/5 */ 1013 return 0; 1014 } 1015 if(!decode_payload_header(stream, clock, header_bytes, size, 0, pkt)) 1016 return 0; 1017 /* check that the length indicated in the payload header is within spec */ 1018 if(pkt->payload_length > max_length) 1019 /* could be encrypted */ 1020 return 1; 1021 bitlength = pkt->payload_length*8; 1022 if(bitlength > size) 1023 return 1; //FIXME should throw exception 1024 1025 unwhiten(stream, pkt->payload, clock, bitlength, 18, pkt); 1026 1027 /* AUX1 has no CRC */ 1028 if (pkt->packet_type == 9) 1029 return 2; 1030 1031 if (payload_crc(pkt)) 1032 return 10; 1033 1034 /* could be encrypted */ 1035 return 2; 1036 } 1037 1038 int EV3(int clock, btbb_packet* pkt) 1039 { 1040 /* skip the access code and packet header */ 1041 char *stream = pkt->symbols + 122; 1042 1043 /* number of symbols remaining after access code and packet header */ 1044 int size = pkt->length - 122; 1045 1046 /* maximum payload length is 30 bytes + 2 bytes CRC */ 1047 int maxlength = 32; 1048 1049 /* number of bits we have decoded */ 1050 int bits; 1051 1052 /* check CRC for any integer byte length up to maxlength */ 1053 for (pkt->payload_length = 0; 1054 pkt->payload_length < maxlength; pkt->payload_length++) { 1055 1056 bits = pkt->payload_length * 8; 1057 1058 /* unwhiten next byte */ 1059 if ((bits + 8) > size) 1060 return 1; //FIXME should throw exception 1061 unwhiten(stream, pkt->payload + bits, clock, 8, 18 + bits, pkt); 1062 1063 if ((pkt->payload_length > 2) && (payload_crc(pkt))) 1064 return 10; 1065 } 1066 return 2; 1067 } 1068 1069 int EV4(int clock, btbb_packet* pkt) 1070 { 1071 char *corrected; 1072 1073 /* skip the access code and packet header */ 1074 char *stream = pkt->symbols + 122; 1075 1076 /* number of symbols remaining after access code and packet header */ 1077 int size = pkt->length - 122; 1078 1079 /* 1080 * maximum payload length is 120 bytes + 2 bytes CRC 1081 * after FEC2/3, this results in a maximum of 1470 symbols 1082 */ 1083 int maxlength = 1470; 1084 1085 /* 1086 * minumum payload length is 1 bytes + 2 bytes CRC 1087 * after FEC2/3, this results in a minimum of 45 symbols 1088 */ 1089 int minlength = 45; 1090 1091 int syms = 0; /* number of symbols we have decoded */ 1092 int bits = 0; /* number of payload bits we have decoded */ 1093 1094 pkt->payload_length = 1; 1095 1096 while (syms < maxlength) { 1097 1098 /* unfec/unwhiten next block (15 symbols -> 10 bits) */ 1099 if (syms + 15 > size) 1100 return 1; //FIXME should throw exception 1101 corrected = unfec23(stream + syms, 10); 1102 if (!corrected) { 1103 free(corrected); 1104 if (syms < minlength) 1105 return 0; 1106 else 1107 return 1; 1108 } 1109 unwhiten(corrected, pkt->payload + bits, clock, 10, 18 + bits, pkt); 1110 free(corrected); 1111 1112 /* check CRC one byte at a time */ 1113 while (pkt->payload_length * 8 <= bits) { 1114 if (payload_crc(pkt)) 1115 return 10; 1116 pkt->payload_length++; 1117 } 1118 syms += 15; 1119 bits += 10; 1120 } 1121 return 2; 1122 } 1123 1124 int EV5(int clock, btbb_packet* pkt) 1125 { 1126 /* skip the access code and packet header */ 1127 char *stream = pkt->symbols + 122; 1128 1129 /* number of symbols remaining after access code and packet header */ 1130 int size = pkt->length - 122; 1131 1132 /* maximum payload length is 180 bytes + 2 bytes CRC */ 1133 int maxlength = 182; 1134 1135 /* number of bits we have decoded */ 1136 int bits; 1137 1138 /* check CRC for any integer byte length up to maxlength */ 1139 for (pkt->payload_length = 0; 1140 pkt->payload_length < maxlength; pkt->payload_length++) { 1141 1142 bits = pkt->payload_length * 8; 1143 1144 /* unwhiten next byte */ 1145 if ((bits + 8) > size) 1146 return 1; //FIXME should throw exception 1147 unwhiten(stream, pkt->payload + bits, clock, 8, 18 + bits, pkt); 1148 1149 if ((pkt->payload_length > 2) && (payload_crc(pkt))) 1150 return 10; 1151 } 1152 return 2; 1153 } 1154 1155 /* HV packet type payload parser */ 1156 int HV(int clock, btbb_packet* pkt) 1157 { 1158 /* skip the access code and packet header */ 1159 char *stream = pkt->symbols + 122; 1160 /* number of symbols remaining after access code and packet header */ 1161 int size = pkt->length - 122; 1162 1163 pkt->payload_header_length = 0; 1164 if(size < 240) { 1165 pkt->payload_length = 0; 1166 return 1; //FIXME should throw exception 1167 } 1168 1169 switch (pkt->packet_type) { 1170 case PACKET_TYPE_HV1: 1171 { 1172 char corrected[80]; 1173 if (!unfec13(stream, corrected, 80)) 1174 return 0; 1175 pkt->payload_length = 10; 1176 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 1); 1177 unwhiten(corrected, pkt->payload, clock, pkt->payload_length*8, 18, pkt); 1178 } 1179 break; 1180 case PACKET_TYPE_HV2: 1181 { 1182 char *corrected = unfec23(stream, 160); 1183 if (!corrected) 1184 return 0; 1185 pkt->payload_length = 20; 1186 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 1); 1187 unwhiten(corrected, pkt->payload, clock, pkt->payload_length*8, 18, pkt); 1188 free(corrected); 1189 } 1190 break; 1191 case PACKET_TYPE_HV3: 1192 pkt->payload_length = 30; 1193 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 1); 1194 unwhiten(stream, pkt->payload, clock, pkt->payload_length*8, 18, pkt); 1195 break; 1196 } 1197 1198 return 2; 1199 } 1200 /* try a clock value (CLK1-6) to unwhiten packet header, 1201 * sets resultant p->packet_type and p->UAP, returns UAP. 1202 */ 1203 uint8_t try_clock(int clock, btbb_packet* pkt) 1204 { 1205 /* skip 72 bit access code */ 1206 char *stream = pkt->symbols + 68; 1207 /* 18 bit packet header */ 1208 char header[18]; 1209 char unwhitened[18]; 1210 1211 if (!unfec13(stream, header, 18)) 1212 return 0; 1213 unwhiten(header, unwhitened, clock, 18, 0, pkt); 1214 uint16_t hdr_data = air_to_host16(unwhitened, 10); 1215 uint8_t hec = air_to_host8(&unwhitened[10], 8); 1216 pkt->UAP = uap_from_hec(hdr_data, hec); 1217 pkt->packet_type = air_to_host8(&unwhitened[3], 4); 1218 1219 return pkt->UAP; 1220 } 1221 1222 /* decode the packet header */ 1223 int btbb_decode_header(btbb_packet* pkt) 1224 { 1225 /* skip 72 bit access code */ 1226 char *stream = pkt->symbols + 68; 1227 /* 18 bit packet header */ 1228 char header[18]; 1229 uint8_t UAP; 1230 1231 if (btbb_packet_get_flag(pkt, BTBB_CLK6_VALID) && unfec13(stream, header, 18)) { 1232 unwhiten(header, pkt->packet_header, pkt->clock, 18, 0, pkt); 1233 uint16_t hdr_data = air_to_host16(pkt->packet_header, 10); 1234 uint8_t hec = air_to_host8(&pkt->packet_header[10], 8); 1235 UAP = uap_from_hec(hdr_data, hec); 1236 if (UAP == pkt->UAP) { 1237 pkt->packet_lt_addr = air_to_host8(&pkt->packet_header[0], 3); 1238 pkt->packet_type = air_to_host8(&pkt->packet_header[3], 4); 1239 pkt->packet_flags = air_to_host8(&pkt->packet_header[7], 3); 1240 pkt->packet_hec = hec; 1241 return 1; 1242 } 1243 } 1244 1245 return 0; 1246 } 1247 1248 int btbb_decode_payload(btbb_packet* pkt) 1249 { 1250 int rv = 0; 1251 pkt->payload_header_length = 0; 1252 1253 switch(pkt->packet_type) 1254 { 1255 case PACKET_TYPE_NULL: 1256 /* no payload to decode */ 1257 pkt->payload_length = 0; 1258 rv = 1; 1259 break; 1260 case PACKET_TYPE_POLL: 1261 /* no payload to decode */ 1262 pkt->payload_length = 0; 1263 rv = 1; 1264 break; 1265 case PACKET_TYPE_FHS: 1266 rv = fhs(pkt->clock, pkt); 1267 break; 1268 case PACKET_TYPE_DM1: 1269 rv = DM(pkt->clock, pkt); 1270 break; 1271 case PACKET_TYPE_DH1: 1272 /* assuming DH1 but could be 2-DH1 */ 1273 rv = DH(pkt->clock, pkt); 1274 break; 1275 case PACKET_TYPE_HV1: 1276 rv = HV(pkt->clock, pkt); 1277 break; 1278 case PACKET_TYPE_HV2: 1279 rv = HV(pkt->clock, pkt); 1280 break; 1281 case PACKET_TYPE_HV3: /* HV3/EV3/3-EV3 */ 1282 /* decode as EV3 if CRC checks out */ 1283 if ((rv = EV3(pkt->clock, pkt)) <= 1) 1284 /* otherwise assume HV3 */ 1285 rv = HV(pkt->clock, pkt); 1286 /* don't know how to decode 3-EV3 */ 1287 break; 1288 case PACKET_TYPE_DV: 1289 /* assuming DV but could be 3-DH1 */ 1290 rv = DM(pkt->clock, pkt); 1291 break; 1292 case PACKET_TYPE_AUX1: 1293 rv = DH(pkt->clock, pkt); 1294 break; 1295 case PACKET_TYPE_DM3: 1296 /* assuming DM3 but could be 2-DH3 */ 1297 rv = DM(pkt->clock, pkt); 1298 break; 1299 case PACKET_TYPE_DH3: 1300 /* assuming DH3 but could be 3-DH3 */ 1301 rv = DH(pkt->clock, pkt); 1302 break; 1303 case PACKET_TYPE_EV4: 1304 /* assuming EV4 but could be 2-EV5 */ 1305 rv = EV4(pkt->clock, pkt); 1306 break; 1307 case PACKET_TYPE_EV5: 1308 /* assuming EV5 but could be 3-EV5 */ 1309 rv = EV5(pkt->clock, pkt); 1310 break; 1311 case PACKET_TYPE_DM5: 1312 /* assuming DM5 but could be 2-DH5 */ 1313 rv = DM(pkt->clock, pkt); 1314 break; 1315 case PACKET_TYPE_DH5: 1316 /* assuming DH5 but could be 3-DH5 */ 1317 rv = DH(pkt->clock, pkt); 1318 break; 1319 } 1320 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 1); 1321 return rv; 1322 } 1323 1324 /* print packet information */ 1325 void btbb_print_packet(const btbb_packet* pkt) 1326 { 1327 if (btbb_packet_get_flag(pkt, BTBB_HAS_PAYLOAD)) { 1328 printf(" Type: %s\n", TYPE_NAMES[pkt->packet_type]); 1329 if (pkt->payload_header_length > 0) { 1330 printf(" LT_ADDR: %d\n", pkt->packet_lt_addr); 1331 printf(" LLID: %d\n", pkt->payload_llid); 1332 printf(" flow: %d\n", pkt->payload_flow); 1333 printf(" payload length: %d\n", pkt->payload_length); 1334 } 1335 if (pkt->payload_length) { 1336 printf(" Data: "); 1337 int i; 1338 for(i=0; i<pkt->payload_length; i++) 1339 printf(" %02x", air_to_host8(pkt->payload + 8*i, 8)); 1340 printf("\n"); 1341 } 1342 } 1343 } 1344 1345 char *tun_format(btbb_packet* pkt) 1346 { 1347 /* include 6 bytes for meta data, 3 bytes for packet header */ 1348 int length = 9 + pkt->payload_length; 1349 char *tun_format = (char *) malloc(length); 1350 int i; 1351 1352 /* meta data */ 1353 tun_format[0] = pkt->clock & 0xff; 1354 tun_format[1] = (pkt->clock >> 8) & 0xff; 1355 tun_format[2] = (pkt->clock >> 16) & 0xff; 1356 tun_format[3] = (pkt->clock >> 24) & 0xff; 1357 tun_format[4] = pkt->channel; 1358 tun_format[5] = btbb_packet_get_flag(pkt, BTBB_CLK27_VALID) | 1359 (btbb_packet_get_flag(pkt, BTBB_NAP_VALID) << 1); 1360 1361 /* packet header modified to fit byte boundaries */ 1362 /* lt_addr and type */ 1363 tun_format[6] = (char) air_to_host8(&pkt->packet_header[0], 7); 1364 /* flags */ 1365 tun_format[7] = (char) air_to_host8(&pkt->packet_header[7], 3); 1366 /* HEC */ 1367 tun_format[8] = (char) air_to_host8(&pkt->packet_header[10], 8); 1368 1369 for(i=0;i<pkt->payload_length;i++) 1370 tun_format[i+9] = (char) air_to_host8(&pkt->payload[i*8], 8); 1371 1372 return tun_format; 1373 } 1374 1375 /* check to see if the packet has a header */ 1376 int btbb_header_present(const btbb_packet* pkt) 1377 { 1378 /* skip to last bit of sync word */ 1379 const char *stream = pkt->symbols + 63; 1380 int be = 0; /* bit errors */ 1381 char msb; /* most significant (last) bit of sync word */ 1382 int a, b, c; 1383 1384 /* check that we have enough symbols */ 1385 if (pkt->length < 122) 1386 return 0; 1387 1388 /* check that the AC trailer is correct */ 1389 msb = stream[0]; 1390 be += stream[1] ^ !msb; 1391 be += stream[2] ^ msb; 1392 be += stream[3] ^ !msb; 1393 be += stream[4] ^ msb; 1394 1395 /* 1396 * Each bit of the 18 bit header is repeated three times. Without 1397 * checking the correctness of any particular bit, just count the 1398 * number of times three symbols in a row don't all agree. 1399 */ 1400 stream += 5; 1401 for (a = 0; a < 54; a += 3) { 1402 b = a + 1; 1403 c = a + 2; 1404 be += ((stream[a] ^ stream[b]) | 1405 (stream[b] ^ stream[c]) | (stream[c] ^ stream[a])); 1406 } 1407 1408 /* 1409 * Few bit errors indicates presence of a header. Many bit errors 1410 * indicates no header is present (i.e. it is an ID packet). 1411 */ 1412 return (be < ID_THRESHOLD); 1413 } 1414 1415 /* extract LAP from FHS payload */ 1416 uint32_t lap_from_fhs(btbb_packet* pkt) 1417 { 1418 /* caller should check got_payload() and get_type() */ 1419 return air_to_host32(&pkt->payload[34], 24); 1420 } 1421 1422 /* extract UAP from FHS payload */ 1423 uint8_t uap_from_fhs(btbb_packet* pkt) 1424 { 1425 /* caller should check got_payload() and get_type() */ 1426 return air_to_host8(&pkt->payload[64], 8); 1427 } 1428 1429 /* extract NAP from FHS payload */ 1430 uint16_t nap_from_fhs(btbb_packet* pkt) 1431 { 1432 /* caller should check got_payload() and get_type() */ 1433 return air_to_host8(&pkt->payload[72], 16); 1434 } 1435 1436 /* extract clock from FHS payload */ 1437 uint32_t clock_from_fhs(btbb_packet* pkt) 1438 { 1439 /* 1440 * caller should check got_payload() and get_type() 1441 * 1442 * This is CLK2-27 (units of 1.25 ms). 1443 * CLK0 and CLK1 are implicitly zero. 1444 */ 1445 return air_to_host32(&pkt->payload[115], 26); 1446 } 1447