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