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