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