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