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