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