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 #define MIN(X, Y) (((X) < (Y)) ? (X) : (Y)) 31 32 /* Maximum number of AC errors supported by library. Caller may 33 * specify any value <= AC_ERROR_LIMIT in btbb_init(). */ 34 #define AC_ERROR_LIMIT 5 35 36 /* maximum number of bit errors for known syncwords */ 37 #define MAX_SYNCWORD_ERRS 5 38 39 /* maximum number of bit errors in */ 40 #define MAX_BARKER_ERRORS 1 41 42 /* default codeword modified for PN sequence and barker code */ 43 #define DEFAULT_CODEWORD 0xb0000002c7820e7eULL 44 45 /* Default access code, used for calculating syndromes */ 46 #define DEFAULT_AC 0xcc7b7268ff614e1bULL 47 48 /* index into whitening data array */ 49 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}; 50 51 /* whitening data */ 52 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}; 53 54 /* lookup table for barker code hamming distance */ 55 static const uint8_t BARKER_DISTANCE[] = { 56 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, 57 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, 58 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, 59 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}; 60 61 /* string representations of packet type */ 62 static const char * const TYPE_NAMES[] = { 63 "NULL", "POLL", "FHS", "DM1", "DH1/2-DH1", "HV1", "HV2/2-EV3", "HV3/EV3/3-EV3", 64 "DV/3-DH1", "AUX1", "DM3/2-DH3", "DH3/3-DH3", "EV4/2-EV5", "EV5/3-EV5", "DM5/2-DH5", "DH5/3-DH5" 65 }; 66 67 /* 68 * generator matrix for sync word (64,30) linear block code 69 * based on polynomial 0260534236651 70 * thanks to http://www.ee.unb.ca/cgi-bin/tervo/polygen.pl 71 * modified for barker code 72 */ 73 static const uint64_t sw_matrix[] = { 74 0xfe000002a0d1c014ULL, 0x01000003f0b9201fULL, 0x008000033ae40edbULL, 0x004000035fca99b9ULL, 75 0x002000036d5dd208ULL, 0x00100001b6aee904ULL, 0x00080000db577482ULL, 0x000400006dabba41ULL, 76 0x00020002f46d43f4ULL, 0x000100017a36a1faULL, 0x00008000bd1b50fdULL, 0x000040029c3536aaULL, 77 0x000020014e1a9b55ULL, 0x0000100265b5d37eULL, 0x0000080132dae9bfULL, 0x000004025bd5ea0bULL, 78 0x00000203ef526bd1ULL, 0x000001033511ab3cULL, 0x000000819a88d59eULL, 0x00000040cd446acfULL, 79 0x00000022a41aabb3ULL, 0x0000001390b5cb0dULL, 0x0000000b0ae27b52ULL, 0x0000000585713da9ULL}; 80 81 static const uint64_t barker_correct[] = { 82 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 83 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 84 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 85 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 86 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 87 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 88 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 89 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 90 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 91 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 92 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 93 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 94 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 95 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 96 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 97 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 98 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 99 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 100 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 101 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 102 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 103 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 104 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 105 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 106 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 107 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 108 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 109 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 110 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL, 111 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 112 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 113 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL}; 114 115 static const uint64_t pn = 0x83848D96BBCC54FCULL; 116 117 static const uint16_t fec23_gen_matrix[] = { 118 0x2c01, 0x5802, 0x1c04, 0x3808, 0x7010, 119 0x4c20, 0x3440, 0x6880, 0x7d00, 0x5600}; 120 121 typedef struct { 122 uint64_t syndrome; /* key */ 123 uint64_t error; 124 UT_hash_handle hh; 125 } syndrome_struct; 126 127 static syndrome_struct *syndrome_map = NULL; 128 129 static void add_syndrome(uint64_t syndrome, uint64_t error) 130 { 131 syndrome_struct *s; 132 s = malloc(sizeof(syndrome_struct)); 133 s->syndrome = syndrome; 134 s->error = error; 135 136 HASH_ADD(hh, syndrome_map, syndrome, 8, s); 137 } 138 139 static syndrome_struct *find_syndrome(uint64_t syndrome) 140 { 141 syndrome_struct *s; 142 143 HASH_FIND(hh, syndrome_map, &syndrome, 8, s); 144 return s; 145 } 146 147 static uint64_t gen_syndrome(uint64_t codeword) 148 { 149 uint64_t syndrome = codeword & 0xffffffff; 150 codeword >>= 32; 151 syndrome ^= sw_check_table4[codeword & 0xff]; 152 codeword >>= 8; 153 syndrome ^= sw_check_table5[codeword & 0xff]; 154 codeword >>= 8; 155 syndrome ^= sw_check_table6[codeword & 0xff]; 156 codeword >>= 8; 157 syndrome ^= sw_check_table7[codeword & 0xff]; 158 return syndrome; 159 } 160 161 static void cycle(uint64_t error, int start, int depth, uint64_t codeword) 162 { 163 uint64_t new_error, syndrome, base; 164 int i; 165 base = 1; 166 depth -= 1; 167 for (i = start; i < 58; i++) 168 { 169 new_error = (base << i); 170 new_error |= error; 171 if (depth) 172 cycle(new_error, i + 1, depth, codeword); 173 else { 174 syndrome = gen_syndrome(codeword ^ new_error); 175 add_syndrome(syndrome, new_error); 176 } 177 } 178 } 179 180 static void gen_syndrome_map(int bit_errors) 181 { 182 int i; 183 for(i = 1; i <= bit_errors; i++) 184 cycle(0, 0, i, DEFAULT_AC); 185 } 186 187 /* Generate Sync Word from an LAP */ 188 uint64_t btbb_gen_syncword(const int LAP) 189 { 190 int i; 191 uint64_t codeword = DEFAULT_CODEWORD; 192 193 /* the sync word generated is in host order, not air order */ 194 for (i = 0; i < 24; i++) 195 if (LAP & (0x800000 >> i)) 196 codeword ^= sw_matrix[i]; 197 198 return codeword; 199 } 200 201 static void init_packet(btbb_packet *pkt, uint32_t lap, uint8_t ac_errors) 202 { 203 pkt->LAP = lap; 204 pkt->ac_errors = ac_errors; 205 206 pkt->flags = 0; 207 btbb_packet_set_flag(pkt, BTBB_WHITENED, 1); 208 } 209 210 /* Convert some number of bits of an air order array to a host order integer */ 211 static uint8_t air_to_host8(const char *air_order, const int bits) 212 { 213 int i; 214 uint8_t host_order = 0; 215 for (i = 0; i < bits; i++) 216 host_order |= ((uint8_t)air_order[i] << i); 217 return host_order; 218 } 219 static uint16_t air_to_host16(const char *air_order, const int bits) 220 { 221 int i; 222 uint16_t host_order = 0; 223 for (i = 0; i < bits; i++) 224 host_order |= ((uint16_t)air_order[i] << i); 225 return host_order; 226 } 227 static uint32_t air_to_host32(const char *air_order, const int bits) 228 { 229 int i; 230 uint32_t host_order = 0; 231 for (i = 0; i < bits; i++) 232 host_order |= ((uint32_t)air_order[i] << i); 233 return host_order; 234 } 235 static uint64_t air_to_host64(const char *air_order, const int bits) 236 { 237 int i; 238 uint64_t host_order = 0; 239 for (i = 0; i < bits; i++) 240 host_order |= ((uint64_t)air_order[i] << i); 241 return host_order; 242 } 243 244 ///* Convert some number of bits in a host order integer to an air order array */ 245 //static void host_to_air(const uint8_t host_order, char *air_order, const int bits) 246 //{ 247 // int i; 248 // for (i = 0; i < bits; i++) 249 // air_order[i] = (host_order >> i) & 0x01; 250 //} 251 252 /* count the number of 1 bits in a uint64_t */ 253 static uint8_t count_bits(uint64_t n) 254 { 255 #ifdef __GNUC__ 256 return (uint8_t) __builtin_popcountll (n); 257 #else 258 uint8_t i = 0; 259 for (i = 0; n != 0; i++) 260 n &= n - 1; 261 return i; 262 #endif 263 } 264 265 #ifndef RELEASE 266 #define RELEASE "unknown" 267 #endif 268 const char* btbb_get_release(void) { 269 return RELEASE; 270 } 271 272 #ifndef VERSION 273 #define VERSION "unknown" 274 #endif 275 const char* btbb_get_version(void) { 276 return VERSION; 277 } 278 279 int btbb_init(int max_ac_errors) 280 { 281 /* Sanity check max_ac_errors. */ 282 if ( (max_ac_errors < 0) || (max_ac_errors > AC_ERROR_LIMIT) ) { 283 fprintf(stderr, "%s: max_ac_errors out of range\n", 284 __FUNCTION__); 285 return -1; 286 } 287 288 if ((syndrome_map == NULL) && (max_ac_errors)) 289 gen_syndrome_map(max_ac_errors); 290 291 return 0; 292 } 293 294 btbb_packet * 295 btbb_packet_new(void) 296 { 297 btbb_packet *pkt = (btbb_packet *)calloc(1, sizeof(btbb_packet)); 298 if(pkt) 299 pkt->refcount = 1; 300 else 301 fprintf(stderr, "Unable to allocate packet"); 302 return pkt; 303 } 304 305 void 306 btbb_packet_ref(btbb_packet *pkt) 307 { 308 pkt->refcount++; 309 } 310 311 void 312 btbb_packet_unref(btbb_packet *pkt) 313 { 314 pkt->refcount--; 315 if (pkt->refcount == 0) 316 free(pkt); 317 } 318 319 uint32_t btbb_packet_get_lap(const btbb_packet *pkt) 320 { 321 return pkt->LAP; 322 } 323 324 void btbb_packet_set_uap(btbb_packet *pkt, uint8_t uap) 325 { 326 pkt->UAP = uap; 327 btbb_packet_set_flag(pkt, BTBB_UAP_VALID, 1); 328 } 329 330 uint8_t btbb_packet_get_uap(const btbb_packet *pkt) 331 { 332 return pkt->UAP; 333 } 334 335 uint16_t btbb_packet_get_nap(const btbb_packet *pkt) 336 { 337 return pkt->NAP; 338 } 339 340 uint32_t btbb_packet_get_clkn(const btbb_packet *pkt) { 341 return pkt->clkn; 342 } 343 344 uint8_t btbb_packet_get_channel(const btbb_packet *pkt) { 345 return pkt->channel; 346 } 347 348 void btbb_packet_set_modulation(btbb_packet *pkt, uint8_t modulation) { 349 pkt->modulation = modulation; 350 } 351 352 uint8_t btbb_packet_get_modulation(const btbb_packet *pkt) { 353 return pkt->modulation; 354 } 355 356 void btbb_packet_set_transport(btbb_packet *pkt, uint8_t transport) { 357 pkt->transport = transport; 358 } 359 360 uint8_t btbb_packet_get_transport(const btbb_packet *pkt) { 361 return pkt->transport; 362 } 363 364 uint8_t btbb_packet_get_ac_errors(const btbb_packet *pkt) { 365 return pkt->ac_errors; 366 } 367 368 int promiscuous_packet_search(char *stream, int search_length, uint32_t *lap, 369 int max_ac_errors, uint8_t *ac_errors) { 370 uint64_t syncword, codeword, syndrome, corrected_barker; 371 syndrome_struct *errors; 372 char *symbols; 373 int count, offset = -1; 374 375 /* Barker code at end of sync word (includes 376 * MSB of LAP) is used as a rough filter. 377 */ 378 uint8_t barker = air_to_host8(&stream[57], 6); 379 barker <<= 1; 380 381 for (count = 0; count < search_length; count++) { 382 symbols = &stream[count]; 383 barker >>= 1; 384 barker |= (symbols[63] << 6); 385 if (BARKER_DISTANCE[barker] <= MAX_BARKER_ERRORS) { 386 // Error correction 387 syncword = air_to_host64(symbols, 64); 388 389 /* correct the barker code with a simple comparison */ 390 corrected_barker = barker_correct[(uint8_t)(syncword >> 57)]; 391 syncword = (syncword & 0x01ffffffffffffffULL) | corrected_barker; 392 393 codeword = syncword ^ pn; 394 395 /* Zero syndrome -> good codeword. */ 396 syndrome = gen_syndrome(codeword); 397 *ac_errors = 0; 398 399 /* Try to fix errors in bad codeword. */ 400 if (syndrome) { 401 errors = find_syndrome(syndrome); 402 if (errors != NULL) { 403 syncword ^= errors->error; 404 *ac_errors = count_bits(errors->error); 405 syndrome = 0; 406 } 407 else { 408 *ac_errors = 0xff; // fail 409 } 410 } 411 412 if (*ac_errors <= max_ac_errors) { 413 *lap = (syncword >> 34) & 0xffffff; 414 offset = count; 415 break; 416 } 417 } 418 } 419 return offset; 420 } 421 422 /* Matching a specific LAP */ 423 int find_known_lap(char *stream, int search_length, uint32_t lap, 424 int max_ac_errors, uint8_t *ac_errors) { 425 uint64_t syncword, ac; 426 char *symbols; 427 int count, offset = -1; 428 429 ac = btbb_gen_syncword(lap); 430 for (count = 0; count < search_length; count++) { 431 symbols = &stream[count]; 432 syncword = air_to_host64(symbols, 64); 433 *ac_errors = count_bits(syncword ^ ac); 434 435 if (*ac_errors <= max_ac_errors) { 436 offset = count; 437 break; 438 } 439 } 440 return offset; 441 } 442 443 /* Looks for an AC in the stream */ 444 int btbb_find_ac(char *stream, int search_length, uint32_t lap, 445 int max_ac_errors, btbb_packet **pkt_ptr) { 446 int offset; 447 uint8_t ac_errors; 448 449 /* Matching any LAP */ 450 if (lap == LAP_ANY) 451 offset = promiscuous_packet_search(stream, search_length, &lap, 452 max_ac_errors, &ac_errors); 453 else 454 offset = find_known_lap(stream, search_length, lap, 455 max_ac_errors, &ac_errors); 456 457 if (offset >= 0) { 458 if (*pkt_ptr == NULL) 459 *pkt_ptr = btbb_packet_new(); 460 init_packet(*pkt_ptr, lap, ac_errors); 461 } 462 463 return offset; 464 } 465 466 /* Copy data (symbols) into packet and set rx data. */ 467 void btbb_packet_set_data(btbb_packet *pkt, char *data, int length, 468 uint8_t channel, uint32_t clkn) 469 { 470 int i; 471 472 if (length > MAX_SYMBOLS) 473 length = MAX_SYMBOLS; 474 for (i = 0; i < length; i++) 475 pkt->symbols[i] = data[i]; 476 477 pkt->length = length; 478 pkt->channel = channel; 479 pkt->clkn = clkn >> 1; // really CLK1 480 } 481 482 void btbb_packet_set_flag(btbb_packet *pkt, int flag, int val) 483 { 484 uint32_t mask = 1L << flag; 485 pkt->flags &= ~mask; 486 if (val) 487 pkt->flags |= mask; 488 } 489 490 int btbb_packet_get_flag(const btbb_packet *pkt, int flag) 491 { 492 uint32_t mask = 1L << flag; 493 return ((pkt->flags & mask) != 0); 494 } 495 496 const char *btbb_get_symbols(const btbb_packet* pkt) 497 { 498 return (const char*) pkt->symbols; 499 } 500 501 int btbb_packet_get_payload_length(const btbb_packet* pkt) 502 { 503 return pkt->payload_length; 504 } 505 506 const char *btbb_get_payload(const btbb_packet* pkt) 507 { 508 return (const char*) pkt->payload; 509 } 510 511 int btbb_get_payload_packed(const btbb_packet* pkt, char *dst) 512 { 513 int i; 514 for(i=0;i<pkt->payload_length;i++) 515 dst[i] = (char) air_to_host8(&pkt->payload[i*8], 8); 516 return pkt->payload_length; 517 } 518 519 uint8_t btbb_packet_get_type(const btbb_packet* pkt) 520 { 521 return pkt->packet_type; 522 } 523 524 uint8_t btbb_packet_get_lt_addr(const btbb_packet* pkt) 525 { 526 return pkt->packet_lt_addr; 527 } 528 529 uint8_t btbb_packet_get_header_flags(const btbb_packet* pkt) 530 { 531 return pkt->packet_flags; 532 } 533 534 uint8_t btbb_packet_get_hec(const btbb_packet* pkt) 535 { 536 return pkt->packet_hec; 537 } 538 539 uint32_t btbb_packet_get_header_packed(const btbb_packet* pkt) 540 { 541 return air_to_host32(&pkt->packet_header[0], 18); 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 PACKET_TYPE_FHS: 721 retval = fhs(clock, pkt); 722 break; 723 724 case PACKET_TYPE_DV: 725 case PACKET_TYPE_DM1: 726 case PACKET_TYPE_DM3: 727 case PACKET_TYPE_DM5: 728 retval = DM(clock, pkt); 729 break; 730 731 case PACKET_TYPE_DH1: 732 case PACKET_TYPE_DH3: 733 case PACKET_TYPE_DH5: 734 retval = DH(clock, pkt); 735 break; 736 737 case PACKET_TYPE_HV3: /* EV3 */ 738 retval = EV3(clock, pkt); 739 break; 740 case PACKET_TYPE_EV4: 741 retval = EV4(clock, pkt); 742 break; 743 case PACKET_TYPE_EV5: 744 retval = EV5(clock, pkt); 745 break; 746 747 case PACKET_TYPE_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 /* Try to set the max payload length to a sensible value, 858 * especially when using strange data 859 */ 860 int max_length = 0; 861 switch(pkt->packet_type) { 862 case PACKET_TYPE_DM1: 863 max_length = 20; 864 break; 865 case PACKET_TYPE_DH1: 866 /* assuming DH1 but could be 2-DH1 (58) */ 867 max_length = 30; 868 break; 869 case PACKET_TYPE_DV: 870 /* assuming DV but could be 3-DH1 (87) */ 871 max_length = 12; /* + 10bytes of voice data */ 872 break; 873 case PACKET_TYPE_DM3: 874 /* assuming DM3 but could be 2-DH3 (371) */ 875 max_length = 125; 876 break; 877 case PACKET_TYPE_DH3: 878 /* assuming DH3 but could be 3-DH3 (556) */ 879 max_length = 187; 880 break; 881 case PACKET_TYPE_DM5: 882 /* assuming DM5 but could be 2-DH5 (683) */ 883 max_length = 228; 884 break; 885 case PACKET_TYPE_DH5: 886 /* assuming DH5 but could be 3-DH5 (1025) */ 887 max_length = 343; 888 break; 889 } 890 pkt->payload_length = MIN(pkt->payload_length, max_length); 891 pkt->payload_llid = air_to_host8(&pkt->payload_header[0], 2); 892 pkt->payload_flow = air_to_host8(&pkt->payload_header[2], 1); 893 pkt->payload_header_length = header_bytes; 894 return 1; 895 } 896 897 /* DM 1/3/5 packet (and DV)*/ 898 int DM(int clock, btbb_packet* pkt) 899 { 900 int bitlength; 901 /* number of bytes in the payload header */ 902 int header_bytes = 2; 903 /* maximum payload length */ 904 int max_length; 905 /* skip the access code and packet header */ 906 char *stream = pkt->symbols + 122; 907 /* number of symbols remaining after access code and packet header */ 908 int size = pkt->length - 122; 909 910 switch(pkt->packet_type) 911 { 912 case PACKET_TYPE_DV: 913 /* skip 80 voice bits, then treat the rest like a DM1 */ 914 stream += 80; 915 size -= 80; 916 header_bytes = 1; 917 /* I don't think the length of the voice field ("synchronous data 918 * field") is included in the length indicated by the payload 919 * header in the data field ("asynchronous data field"), but I 920 * could be wrong. 921 */ 922 max_length = 12; 923 break; 924 case PACKET_TYPE_DM1: 925 header_bytes = 1; 926 max_length = 20; 927 break; 928 case PACKET_TYPE_DM3: 929 max_length = 125; 930 break; 931 case PACKET_TYPE_DM5: 932 max_length = 228; 933 break; 934 default: /* not a DM1/3/5 or DV */ 935 return 0; 936 } 937 if(!decode_payload_header(stream, clock, header_bytes, size, 1, pkt)) 938 return 0; 939 /* check that the length indicated in the payload header is within spec */ 940 if(pkt->payload_length > max_length) 941 /* could be encrypted */ 942 return 1; 943 bitlength = pkt->payload_length*8; 944 if(bitlength > size) 945 return 1; //FIXME should throw exception 946 947 char *corrected = unfec23(stream, bitlength); 948 if (!corrected) 949 return 0; 950 unwhiten(corrected, pkt->payload, clock, bitlength, 18, pkt); 951 free(corrected); 952 953 if (payload_crc(pkt)) 954 return 10; 955 956 /* could be encrypted */ 957 return 2; 958 } 959 960 /* DH 1/3/5 packet (and AUX1) */ 961 /* similar to DM 1/3/5 but without FEC */ 962 int DH(int clock, btbb_packet* pkt) 963 { 964 int bitlength; 965 /* number of bytes in the payload header */ 966 int header_bytes = 2; 967 /* maximum payload length */ 968 int max_length; 969 /* skip the access code and packet header */ 970 char *stream = pkt->symbols + 122; 971 /* number of symbols remaining after access code and packet header */ 972 int size = pkt->length - 122; 973 974 switch(pkt->packet_type) 975 { 976 case PACKET_TYPE_AUX1: 977 case PACKET_TYPE_DH1: 978 header_bytes = 1; 979 max_length = 30; 980 break; 981 case PACKET_TYPE_DH3: 982 max_length = 187; 983 break; 984 case PACKET_TYPE_DH5: 985 max_length = 343; 986 break; 987 default: /* not a DH1/3/5 */ 988 return 0; 989 } 990 if(!decode_payload_header(stream, clock, header_bytes, size, 0, pkt)) 991 return 0; 992 /* check that the length indicated in the payload header is within spec */ 993 if(pkt->payload_length > max_length) 994 /* could be encrypted */ 995 return 1; 996 bitlength = pkt->payload_length*8; 997 if(bitlength > size) 998 return 1; //FIXME should throw exception 999 1000 unwhiten(stream, pkt->payload, clock, bitlength, 18, pkt); 1001 1002 /* AUX1 has no CRC */ 1003 if (pkt->packet_type == 9) 1004 return 2; 1005 1006 if (payload_crc(pkt)) 1007 return 10; 1008 1009 /* could be encrypted */ 1010 return 2; 1011 } 1012 1013 int EV3(int clock, btbb_packet* pkt) 1014 { 1015 /* skip the access code and packet header */ 1016 char *stream = pkt->symbols + 122; 1017 1018 /* number of symbols remaining after access code and packet header */ 1019 int size = pkt->length - 122; 1020 1021 /* maximum payload length is 30 bytes + 2 bytes CRC */ 1022 int maxlength = 32; 1023 1024 /* number of bits we have decoded */ 1025 int bits; 1026 1027 /* check CRC for any integer byte length up to maxlength */ 1028 for (pkt->payload_length = 0; 1029 pkt->payload_length < maxlength; pkt->payload_length++) { 1030 1031 bits = pkt->payload_length * 8; 1032 1033 /* unwhiten next byte */ 1034 if ((bits + 8) > size) 1035 return 1; //FIXME should throw exception 1036 unwhiten(stream, pkt->payload + bits, clock, 8, 18 + bits, pkt); 1037 1038 if ((pkt->payload_length > 2) && (payload_crc(pkt))) 1039 return 10; 1040 } 1041 return 2; 1042 } 1043 1044 int EV4(int clock, btbb_packet* pkt) 1045 { 1046 char *corrected; 1047 1048 /* skip the access code and packet header */ 1049 char *stream = pkt->symbols + 122; 1050 1051 /* number of symbols remaining after access code and packet header */ 1052 int size = pkt->length - 122; 1053 1054 /* 1055 * maximum payload length is 120 bytes + 2 bytes CRC 1056 * after FEC2/3, this results in a maximum of 1470 symbols 1057 */ 1058 int maxlength = 1470; 1059 1060 /* 1061 * minumum payload length is 1 bytes + 2 bytes CRC 1062 * after FEC2/3, this results in a minimum of 45 symbols 1063 */ 1064 int minlength = 45; 1065 1066 int syms = 0; /* number of symbols we have decoded */ 1067 int bits = 0; /* number of payload bits we have decoded */ 1068 1069 pkt->payload_length = 1; 1070 1071 while (syms < maxlength) { 1072 1073 /* unfec/unwhiten next block (15 symbols -> 10 bits) */ 1074 if (syms + 15 > size) 1075 return 1; //FIXME should throw exception 1076 corrected = unfec23(stream + syms, 10); 1077 if (!corrected) { 1078 free(corrected); 1079 if (syms < minlength) 1080 return 0; 1081 else 1082 return 1; 1083 } 1084 unwhiten(corrected, pkt->payload + bits, clock, 10, 18 + bits, pkt); 1085 free(corrected); 1086 1087 /* check CRC one byte at a time */ 1088 while (pkt->payload_length * 8 <= bits) { 1089 if (payload_crc(pkt)) 1090 return 10; 1091 pkt->payload_length++; 1092 } 1093 syms += 15; 1094 bits += 10; 1095 } 1096 return 2; 1097 } 1098 1099 int EV5(int clock, btbb_packet* pkt) 1100 { 1101 /* skip the access code and packet header */ 1102 char *stream = pkt->symbols + 122; 1103 1104 /* number of symbols remaining after access code and packet header */ 1105 int size = pkt->length - 122; 1106 1107 /* maximum payload length is 180 bytes + 2 bytes CRC */ 1108 int maxlength = 182; 1109 1110 /* number of bits we have decoded */ 1111 int bits; 1112 1113 /* check CRC for any integer byte length up to maxlength */ 1114 for (pkt->payload_length = 0; 1115 pkt->payload_length < maxlength; pkt->payload_length++) { 1116 1117 bits = pkt->payload_length * 8; 1118 1119 /* unwhiten next byte */ 1120 if ((bits + 8) > size) 1121 return 1; //FIXME should throw exception 1122 unwhiten(stream, pkt->payload + bits, clock, 8, 18 + bits, pkt); 1123 1124 if ((pkt->payload_length > 2) && (payload_crc(pkt))) 1125 return 10; 1126 } 1127 return 2; 1128 } 1129 1130 /* HV packet type payload parser */ 1131 int HV(int clock, btbb_packet* pkt) 1132 { 1133 /* skip the access code and packet header */ 1134 char *stream = pkt->symbols + 122; 1135 /* number of symbols remaining after access code and packet header */ 1136 int size = pkt->length - 122; 1137 1138 pkt->payload_header_length = 0; 1139 if(size < 240) { 1140 pkt->payload_length = 0; 1141 return 1; //FIXME should throw exception 1142 } 1143 1144 switch (pkt->packet_type) { 1145 case PACKET_TYPE_HV1: 1146 { 1147 char corrected[80]; 1148 if (!unfec13(stream, corrected, 80)) 1149 return 0; 1150 pkt->payload_length = 10; 1151 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 1); 1152 unwhiten(corrected, pkt->payload, clock, pkt->payload_length*8, 18, pkt); 1153 } 1154 break; 1155 case PACKET_TYPE_HV2: 1156 { 1157 char *corrected = unfec23(stream, 160); 1158 if (!corrected) 1159 return 0; 1160 pkt->payload_length = 20; 1161 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 1); 1162 unwhiten(corrected, pkt->payload, clock, pkt->payload_length*8, 18, pkt); 1163 free(corrected); 1164 } 1165 break; 1166 case PACKET_TYPE_HV3: 1167 pkt->payload_length = 30; 1168 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 1); 1169 unwhiten(stream, pkt->payload, clock, pkt->payload_length*8, 18, pkt); 1170 break; 1171 } 1172 1173 return 2; 1174 } 1175 /* try a clock value (CLK1-6) to unwhiten packet header, 1176 * sets resultant p->packet_type and p->UAP, returns UAP. 1177 */ 1178 uint8_t try_clock(int clock, btbb_packet* pkt) 1179 { 1180 /* skip 72 bit access code */ 1181 char *stream = pkt->symbols + 68; 1182 /* 18 bit packet header */ 1183 char header[18]; 1184 char unwhitened[18]; 1185 1186 if (!unfec13(stream, header, 18)) 1187 return 0; 1188 unwhiten(header, unwhitened, clock, 18, 0, pkt); 1189 uint16_t hdr_data = air_to_host16(unwhitened, 10); 1190 uint8_t hec = air_to_host8(&unwhitened[10], 8); 1191 pkt->UAP = uap_from_hec(hdr_data, hec); 1192 pkt->packet_type = air_to_host8(&unwhitened[3], 4); 1193 1194 return pkt->UAP; 1195 } 1196 1197 /* decode the packet header */ 1198 int btbb_decode_header(btbb_packet* pkt) 1199 { 1200 /* skip 72 bit access code */ 1201 char *stream = pkt->symbols + 68; 1202 /* 18 bit packet header */ 1203 char header[18]; 1204 uint8_t UAP; 1205 1206 if (btbb_packet_get_flag(pkt, BTBB_CLK6_VALID) && unfec13(stream, header, 18)) { 1207 unwhiten(header, pkt->packet_header, pkt->clkn, 18, 0, pkt); 1208 uint16_t hdr_data = air_to_host16(pkt->packet_header, 10); 1209 uint8_t hec = air_to_host8(&pkt->packet_header[10], 8); 1210 UAP = uap_from_hec(hdr_data, hec); 1211 if (UAP == pkt->UAP) { 1212 pkt->packet_lt_addr = air_to_host8(&pkt->packet_header[0], 3); 1213 pkt->packet_type = air_to_host8(&pkt->packet_header[3], 4); 1214 pkt->packet_flags = air_to_host8(&pkt->packet_header[7], 3); 1215 pkt->packet_hec = hec; 1216 return 1; 1217 } 1218 } 1219 1220 return 0; 1221 } 1222 1223 int btbb_decode_payload(btbb_packet* pkt) 1224 { 1225 int rv = 0; 1226 pkt->payload_header_length = 0; 1227 1228 switch(pkt->packet_type) 1229 { 1230 case PACKET_TYPE_NULL: 1231 /* no payload to decode */ 1232 pkt->payload_length = 0; 1233 rv = 1; 1234 break; 1235 case PACKET_TYPE_POLL: 1236 /* no payload to decode */ 1237 pkt->payload_length = 0; 1238 rv = 1; 1239 break; 1240 case PACKET_TYPE_FHS: 1241 rv = fhs(pkt->clkn, pkt); 1242 break; 1243 case PACKET_TYPE_DM1: 1244 rv = DM(pkt->clkn, pkt); 1245 break; 1246 case PACKET_TYPE_DH1: 1247 /* assuming DH1 but could be 2-DH1 */ 1248 rv = DH(pkt->clkn, pkt); 1249 break; 1250 case PACKET_TYPE_HV1: 1251 rv = HV(pkt->clkn, pkt); 1252 break; 1253 case PACKET_TYPE_HV2: 1254 rv = HV(pkt->clkn, pkt); 1255 break; 1256 case PACKET_TYPE_HV3: /* HV3/EV3/3-EV3 */ 1257 /* decode as EV3 if CRC checks out */ 1258 if ((rv = EV3(pkt->clkn, pkt)) <= 1) 1259 /* otherwise assume HV3 */ 1260 rv = HV(pkt->clkn, pkt); 1261 /* don't know how to decode 3-EV3 */ 1262 break; 1263 case PACKET_TYPE_DV: 1264 /* assuming DV but could be 3-DH1 */ 1265 rv = DM(pkt->clkn, pkt); 1266 break; 1267 case PACKET_TYPE_AUX1: 1268 rv = DH(pkt->clkn, pkt); 1269 break; 1270 case PACKET_TYPE_DM3: 1271 /* assuming DM3 but could be 2-DH3 */ 1272 rv = DM(pkt->clkn, pkt); 1273 break; 1274 case PACKET_TYPE_DH3: 1275 /* assuming DH3 but could be 3-DH3 */ 1276 rv = DH(pkt->clkn, pkt); 1277 break; 1278 case PACKET_TYPE_EV4: 1279 /* assuming EV4 but could be 2-EV5 */ 1280 rv = EV4(pkt->clkn, pkt); 1281 break; 1282 case PACKET_TYPE_EV5: 1283 /* assuming EV5 but could be 3-EV5 */ 1284 rv = EV5(pkt->clkn, pkt); 1285 break; 1286 case PACKET_TYPE_DM5: 1287 /* assuming DM5 but could be 2-DH5 */ 1288 rv = DM(pkt->clkn, pkt); 1289 break; 1290 case PACKET_TYPE_DH5: 1291 /* assuming DH5 but could be 3-DH5 */ 1292 rv = DH(pkt->clkn, pkt); 1293 break; 1294 } 1295 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 1); 1296 return rv; 1297 } 1298 1299 /* decode the whole packet */ 1300 int btbb_decode(btbb_packet* pkt) 1301 { 1302 int rv = 0; 1303 1304 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 0); 1305 1306 if (btbb_decode_header(pkt)) { 1307 rv = btbb_decode_payload(pkt); 1308 } 1309 1310 /* If we were successful, print the packet */ 1311 if(rv > 0) { 1312 printf("Packet decoded with clock 0x%02x (rv=%d)\n", pkt->clkn & 0x3f, rv); 1313 btbb_print_packet(pkt); 1314 } 1315 1316 return rv; 1317 } 1318 1319 /* print packet information */ 1320 void btbb_print_packet(const btbb_packet* pkt) 1321 { 1322 if (btbb_packet_get_flag(pkt, BTBB_HAS_PAYLOAD)) { 1323 printf(" Type: %s\n", TYPE_NAMES[pkt->packet_type]); 1324 if (pkt->payload_header_length > 0) { 1325 printf(" LT_ADDR: %d\n", pkt->packet_lt_addr); 1326 printf(" LLID: %d\n", pkt->payload_llid); 1327 printf(" flow: %d\n", pkt->payload_flow); 1328 printf(" payload length: %d\n", pkt->payload_length); 1329 } 1330 if (pkt->payload_length) { 1331 printf(" Data: "); 1332 int i; 1333 for(i=0; i<pkt->payload_length; i++) 1334 printf(" %02x", air_to_host8(pkt->payload + 8*i, 8)); 1335 printf("\n"); 1336 } 1337 } 1338 } 1339 1340 char *tun_format(btbb_packet* pkt) 1341 { 1342 /* include 6 bytes for meta data, 3 bytes for packet header */ 1343 int length = 9 + pkt->payload_length; 1344 char *tun_format = (char *) malloc(length); 1345 int i; 1346 1347 /* meta data */ 1348 tun_format[0] = pkt->clkn & 0xff; 1349 tun_format[1] = (pkt->clkn >> 8) & 0xff; 1350 tun_format[2] = (pkt->clkn >> 16) & 0xff; 1351 tun_format[3] = (pkt->clkn >> 24) & 0xff; 1352 tun_format[4] = pkt->channel; 1353 tun_format[5] = btbb_packet_get_flag(pkt, BTBB_CLK27_VALID) | 1354 (btbb_packet_get_flag(pkt, BTBB_NAP_VALID) << 1); 1355 1356 /* packet header modified to fit byte boundaries */ 1357 /* lt_addr and type */ 1358 tun_format[6] = (char) air_to_host8(&pkt->packet_header[0], 7); 1359 /* flags */ 1360 tun_format[7] = (char) air_to_host8(&pkt->packet_header[7], 3); 1361 /* HEC */ 1362 tun_format[8] = (char) air_to_host8(&pkt->packet_header[10], 8); 1363 1364 for(i=0;i<pkt->payload_length;i++) 1365 tun_format[i+9] = (char) air_to_host8(&pkt->payload[i*8], 8); 1366 1367 return tun_format; 1368 } 1369 1370 /* check to see if the packet has a header */ 1371 int btbb_header_present(const btbb_packet* pkt) 1372 { 1373 /* skip to last bit of sync word */ 1374 const char *stream = pkt->symbols + 63; 1375 int be = 0; /* bit errors */ 1376 char msb; /* most significant (last) bit of sync word */ 1377 int a, b, c; 1378 1379 /* check that we have enough symbols */ 1380 if (pkt->length < 122) 1381 return 0; 1382 1383 /* check that the AC trailer is correct */ 1384 msb = stream[0]; 1385 be += stream[1] ^ !msb; 1386 be += stream[2] ^ msb; 1387 be += stream[3] ^ !msb; 1388 be += stream[4] ^ msb; 1389 1390 /* 1391 * Each bit of the 18 bit header is repeated three times. Without 1392 * checking the correctness of any particular bit, just count the 1393 * number of times three symbols in a row don't all agree. 1394 */ 1395 stream += 5; 1396 for (a = 0; a < 54; a += 3) { 1397 b = a + 1; 1398 c = a + 2; 1399 be += ((stream[a] ^ stream[b]) | 1400 (stream[b] ^ stream[c]) | (stream[c] ^ stream[a])); 1401 } 1402 1403 /* 1404 * Few bit errors indicates presence of a header. Many bit errors 1405 * indicates no header is present (i.e. it is an ID packet). 1406 */ 1407 return (be < ID_THRESHOLD); 1408 } 1409 1410 /* extract LAP from FHS payload */ 1411 uint32_t lap_from_fhs(btbb_packet* pkt) 1412 { 1413 /* caller should check got_payload() and get_type() */ 1414 return air_to_host32(&pkt->payload[34], 24); 1415 } 1416 1417 /* extract UAP from FHS payload */ 1418 uint8_t uap_from_fhs(btbb_packet* pkt) 1419 { 1420 /* caller should check got_payload() and get_type() */ 1421 return air_to_host8(&pkt->payload[64], 8); 1422 } 1423 1424 /* extract NAP from FHS payload */ 1425 uint16_t nap_from_fhs(btbb_packet* pkt) 1426 { 1427 /* caller should check got_payload() and get_type() */ 1428 return air_to_host16(&pkt->payload[72], 16); 1429 } 1430 1431 /* extract clock from FHS payload */ 1432 uint32_t clock_from_fhs(btbb_packet* pkt) 1433 { 1434 /* 1435 * caller should check got_payload() and get_type() 1436 * 1437 * This is CLK2-27 (units of 1.25 ms). 1438 * CLK0 and CLK1 are implicitly zero. 1439 */ 1440 return air_to_host32(&pkt->payload[115], 26); 1441 } 1442