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