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