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