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