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