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