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 "bluetooth_packet.h" 24 #include "bluetooth_piconet.h" 25 #include "uthash.h" 26 #include <stdlib.h> 27 #include <stdio.h> 28 29 int perm_table_initialized = 0; 30 char perm_table[0x20][0x20][0x200]; 31 32 btbb_piconet * 33 btbb_piconet_new(void) 34 { 35 btbb_piconet *pn = (btbb_piconet *)calloc(1, sizeof(btbb_piconet)); 36 pn->refcount = 1; 37 return pn; 38 } 39 40 void 41 btbb_piconet_ref(btbb_piconet *pn) 42 { 43 pn->refcount++; 44 } 45 46 void 47 btbb_piconet_unref(btbb_piconet *pn) 48 { 49 pn->refcount--; 50 if (pn->refcount == 0) 51 free(pn); 52 } 53 54 void btbb_init_piconet(btbb_piconet *pn, uint32_t lap) 55 { 56 pn->LAP = lap; 57 btbb_piconet_set_flag(pn, BTBB_LAP_VALID, 1); 58 } 59 60 void btbb_piconet_set_flag(btbb_piconet *pn, int flag, int val) 61 { 62 uint32_t mask = 1L << flag; 63 pn->flags &= ~mask; 64 if (val) 65 pn->flags |= mask; 66 } 67 68 int btbb_piconet_get_flag(const btbb_piconet *pn, const int flag) 69 { 70 uint32_t mask = 1L << flag; 71 return ((pn->flags & mask) != 0); 72 } 73 74 void btbb_piconet_set_uap(btbb_piconet *pn, uint8_t uap) 75 { 76 pn->UAP = uap; 77 btbb_piconet_set_flag(pn, BTBB_UAP_VALID, 1); 78 } 79 80 uint8_t btbb_piconet_get_uap(const btbb_piconet *pn) 81 { 82 return pn->UAP; 83 } 84 85 uint32_t btbb_piconet_get_lap(const btbb_piconet *pn) 86 { 87 return pn->LAP; 88 } 89 90 uint16_t btbb_piconet_get_nap(const btbb_piconet *pn) 91 { 92 return pn->NAP; 93 } 94 95 int btbb_piconet_get_clk_offset(const btbb_piconet *pn) 96 { 97 return pn->clk_offset; 98 } 99 100 void btbb_piconet_set_clk_offset(btbb_piconet *pn, int clk_offset) 101 { 102 pn->clk_offset = clk_offset; 103 } 104 105 void btbb_piconet_set_afh_map(btbb_piconet *pn, uint8_t *afh_map) { 106 int i; 107 // DGS: Unroll this? 108 for(i=0; i<10; i++) 109 pn->afh_map[i] = afh_map[i]; 110 } 111 112 uint8_t *btbb_piconet_get_afh_map(btbb_piconet *pn) { 113 return pn->afh_map; 114 } 115 116 void btbb_piconet_set_channel_seen(btbb_piconet *pn, uint8_t channel) 117 { 118 pn->afh_map[channel/8] |= 0x1 << (channel % 8); 119 } 120 121 /* do all the precalculation that can be done before knowing the address */ 122 void precalc(btbb_piconet *pn) 123 { 124 int i; 125 126 /* populate frequency register bank*/ 127 for (i = 0; i < BT_NUM_CHANNELS; i++) 128 pn->bank[i] = ((i * 2) % BT_NUM_CHANNELS); 129 /* actual frequency is 2402 + pn->bank[i] MHz */ 130 131 } 132 133 /* do precalculation that requires the address */ 134 void address_precalc(int address, btbb_piconet *pn) 135 { 136 /* precalculate some of single_hop()/gen_hop()'s variables */ 137 pn->a1 = (address >> 23) & 0x1f; 138 pn->b = (address >> 19) & 0x0f; 139 pn->c1 = ((address >> 4) & 0x10) + 140 ((address >> 3) & 0x08) + 141 ((address >> 2) & 0x04) + 142 ((address >> 1) & 0x02) + 143 (address & 0x01); 144 pn->d1 = (address >> 10) & 0x1ff; 145 pn->e = ((address >> 7) & 0x40) + 146 ((address >> 6) & 0x20) + 147 ((address >> 5) & 0x10) + 148 ((address >> 4) & 0x08) + 149 ((address >> 3) & 0x04) + 150 ((address >> 2) & 0x02) + 151 ((address >> 1) & 0x01); 152 } 153 154 #ifdef WC4 155 /* These are optimization experiments, which don't help much for 156 * x86. Hold on to them to see whether they're useful on ARM. */ 157 158 #ifdef NEVER 159 #define BUTTERFLY(z,p,c,a,b) \ 160 if ( ((p&(1<<c))!=0) & (((z&(1<<a))!=0) ^ ((z&(1<<b))!=0)) ) \ 161 z ^= ((1<<a)|(1<<b)) 162 #endif 163 164 #define BUTTERFLY(z,p,c,a,b) \ 165 if ( (((z>>a)^(z>>b)) & (p>>c)) & 0x1 ) \ 166 z ^= ((1<<a)|(1<<b)) 167 168 int perm5(int z, int p_high, int p_low) 169 { 170 int p = (p_high << 5) | p_low; 171 BUTTERFLY(z,p,13,1,2); 172 BUTTERFLY(z,p,12,0,3); 173 BUTTERFLY(z,p,11,1,3); 174 BUTTERFLY(z,p,10,2,4); 175 BUTTERFLY(z,p, 9,0,3); 176 BUTTERFLY(z,p, 8,1,4); 177 BUTTERFLY(z,p, 7,3,4); 178 BUTTERFLY(z,p, 6,0,2); 179 BUTTERFLY(z,p, 5,1,3); 180 BUTTERFLY(z,p, 4,0,4); 181 BUTTERFLY(z,p, 3,3,4); 182 BUTTERFLY(z,p, 2,1,2); 183 BUTTERFLY(z,p, 1,2,3); 184 BUTTERFLY(z,p, 0,0,1); 185 186 return z; 187 } 188 #endif // WC4 189 190 /* 5 bit permutation */ 191 /* assumes z is constrained to 5 bits, p_high to 5 bits, p_low to 9 bits */ 192 int perm5(int z, int p_high, int p_low) 193 { 194 int i, tmp, output, z_bit[5], p[14]; 195 int index1[] = {0, 2, 1, 3, 0, 1, 0, 3, 1, 0, 2, 1, 0, 1}; 196 int index2[] = {1, 3, 2, 4, 4, 3, 2, 4, 4, 3, 4, 3, 3, 2}; 197 198 /* bits of p_low and p_high are control signals */ 199 for (i = 0; i < 9; i++) 200 p[i] = (p_low >> i) & 0x01; 201 for (i = 0; i < 5; i++) 202 p[i+9] = (p_high >> i) & 0x01; 203 204 /* bit swapping will be easier with an array of bits */ 205 for (i = 0; i < 5; i++) 206 z_bit[i] = (z >> i) & 0x01; 207 208 /* butterfly operations */ 209 for (i = 13; i >= 0; i--) { 210 /* swap bits according to index arrays if control signal tells us to */ 211 if (p[i]) { 212 tmp = z_bit[index1[i]]; 213 z_bit[index1[i]] = z_bit[index2[i]]; 214 z_bit[index2[i]] = tmp; 215 } 216 } 217 218 /* reconstruct output from rearranged bits */ 219 output = 0; 220 for (i = 0; i < 5; i++) 221 output += z_bit[i] << i; 222 223 return(output); 224 } 225 226 void perm_table_init(void) 227 { 228 /* populate perm_table for all possible inputs */ 229 int z, p_high, p_low; 230 for (z = 0; z < 0x20; z++) 231 for (p_high = 0; p_high < 0x20; p_high++) 232 for (p_low = 0; p_low < 0x200; p_low++) 233 perm_table[z][p_high][p_low] = perm5(z, p_high, p_low); 234 } 235 236 /* drop-in replacement for perm5() using lookup table */ 237 int fast_perm(int z, int p_high, int p_low) 238 { 239 if (!perm_table_initialized) { 240 perm_table_init(); 241 perm_table_initialized = 1; 242 } 243 244 return(perm_table[z][p_high][p_low]); 245 } 246 247 /* generate the complete hopping sequence */ 248 static void gen_hops(btbb_piconet *pn) 249 { 250 /* a, b, c, d, e, f, x, y1, y2 are variable names used in section 2.6 of the spec */ 251 /* b is already defined */ 252 /* e is already defined */ 253 int a, c, d, f, x; 254 int h, i, j, k, c_flipped, perm_in, perm_out; 255 256 /* sequence index = clock >> 1 */ 257 /* (hops only happen at every other clock value) */ 258 int index = 0; 259 f = 0; 260 261 /* nested loops for optimization (not recalculating every variable with every clock tick) */ 262 for (h = 0; h < 0x04; h++) { /* clock bits 26-27 */ 263 for (i = 0; i < 0x20; i++) { /* clock bits 21-25 */ 264 a = pn->a1 ^ i; 265 for (j = 0; j < 0x20; j++) { /* clock bits 16-20 */ 266 c = pn->c1 ^ j; 267 c_flipped = c ^ 0x1f; 268 for (k = 0; k < 0x200; k++) { /* clock bits 7-15 */ 269 d = pn->d1 ^ k; 270 for (x = 0; x < 0x20; x++) { /* clock bits 2-6 */ 271 perm_in = ((x + a) % 32) ^ pn->b; 272 /* y1 (clock bit 1) = 0, y2 = 0 */ 273 perm_out = fast_perm(perm_in, c, d); 274 pn->sequence[index] = pn->bank[(perm_out + pn->e + f) % BT_NUM_CHANNELS]; 275 if (btbb_piconet_get_flag(pn, BTBB_IS_AFH)) { 276 pn->sequence[index + 1] = pn->sequence[index]; 277 } else { 278 /* y1 (clock bit 1) = 1, y2 = 32 */ 279 perm_out = fast_perm(perm_in, c_flipped, d); 280 pn->sequence[index + 1] = pn->bank[(perm_out + pn->e + f + 32) % BT_NUM_CHANNELS]; 281 } 282 index += 2; 283 } 284 f += 16; 285 } 286 } 287 } 288 } 289 } 290 291 /* Function to calculate piconet hopping patterns and add to hash map */ 292 void gen_hop_pattern(btbb_piconet *pn) 293 { 294 printf("\nCalculating complete hopping sequence.\n"); 295 /* this holds the entire hopping sequence */ 296 pn->sequence = (char*) malloc(SEQUENCE_LENGTH); 297 298 precalc(pn); 299 address_precalc(((pn->UAP<<24) | pn->LAP) & 0xfffffff, pn); 300 gen_hops(pn); 301 302 printf("Hopping sequence calculated.\n"); 303 } 304 305 /* Container for hopping pattern */ 306 typedef struct { 307 uint64_t key; /* afh flag + address */ 308 char *sequence; 309 UT_hash_handle hh; 310 } hopping_struct; 311 312 static hopping_struct *hopping_map = NULL; 313 314 /* Function to fetch piconet hopping patterns */ 315 void get_hop_pattern(btbb_piconet *pn) 316 { 317 hopping_struct *s; 318 uint64_t key; 319 320 /* Two stages to avoid "left shift count >= width of type" warning */ 321 key = btbb_piconet_get_flag(pn, BTBB_IS_AFH); 322 key = (key<<32) | (pn->UAP<<24) | pn->LAP; 323 HASH_FIND(hh, hopping_map, &key, 4, s); 324 325 if (s == NULL) { 326 gen_hop_pattern(pn); 327 s = malloc(sizeof(hopping_struct)); 328 s->key = key; 329 s->sequence = pn->sequence; 330 HASH_ADD(hh, hopping_map, key, 4, s); 331 } else { 332 printf("\nFound hopping sequence in cache.\n"); 333 pn->sequence = s->sequence; 334 } 335 } 336 337 /* determine channel for a particular hop */ 338 /* replaced with gen_hops() for a complete sequence but could still come in handy */ 339 char single_hop(int clock, btbb_piconet *pn) 340 { 341 int a, c, d, f, x, y1, y2; 342 343 /* following variable names used in section 2.6 of the spec */ 344 x = (clock >> 2) & 0x1f; 345 y1 = (clock >> 1) & 0x01; 346 y2 = y1 << 5; 347 a = (pn->a1 ^ (clock >> 21)) & 0x1f; 348 /* b is already defined */ 349 c = (pn->c1 ^ (clock >> 16)) & 0x1f; 350 d = (pn->d1 ^ (clock >> 7)) & 0x1ff; 351 /* e is already defined */ 352 f = (clock >> 3) & 0x1fffff0; 353 354 /* hop selection */ 355 return(pn->bank[(fast_perm(((x + a) % 32) ^ pn->b, (y1 * 0x1f) ^ c, d) + pn->e + f + y2) % BT_NUM_CHANNELS]); 356 } 357 358 /* look up channel for a particular hop */ 359 char hop(int clock, btbb_piconet *pn) 360 { 361 return pn->sequence[clock]; 362 } 363 364 static char aliased_channel(char channel) 365 { 366 return ((channel + 24) % ALIASED_CHANNELS) + 26; 367 } 368 369 /* create list of initial candidate clock values (hops with same channel as first observed hop) */ 370 static int init_candidates(char channel, int known_clock_bits, btbb_piconet *pn) 371 { 372 int i; 373 int count = 0; /* total number of candidates */ 374 char observable_channel; /* accounts for aliasing if necessary */ 375 376 /* only try clock values that match our known bits */ 377 for (i = known_clock_bits; i < SEQUENCE_LENGTH; i += 0x40) { 378 if (pn->aliased) 379 observable_channel = aliased_channel(pn->sequence[i]); 380 else 381 observable_channel = pn->sequence[i]; 382 if (observable_channel == channel) 383 pn->clock_candidates[count++] = i; 384 //FIXME ought to throw exception if count gets too big 385 } 386 return count; 387 } 388 389 /* initialize the hop reversal process */ 390 int btbb_init_hop_reversal(int aliased, btbb_piconet *pn) 391 { 392 int max_candidates; 393 uint32_t clock; 394 395 get_hop_pattern(pn); 396 397 if(aliased) 398 max_candidates = (SEQUENCE_LENGTH / ALIASED_CHANNELS) / 32; 399 else 400 max_candidates = (SEQUENCE_LENGTH / BT_NUM_CHANNELS) / 32; 401 /* this can hold twice the approximate number of initial candidates */ 402 pn->clock_candidates = (uint32_t*) malloc(sizeof(uint32_t) * max_candidates); 403 404 clock = (pn->clk_offset + pn->first_pkt_time) & 0x3f; 405 pn->num_candidates = init_candidates(pn->pattern_channels[0], clock, pn); 406 pn->winnowed = 0; 407 btbb_piconet_set_flag(pn, BTBB_HOP_REVERSAL_INIT, 1); 408 btbb_piconet_set_flag(pn, BTBB_CLK27_VALID, 0); 409 btbb_piconet_set_flag(pn, BTBB_IS_ALIASED, aliased); 410 411 printf("%d initial CLK1-27 candidates\n", pn->num_candidates); 412 413 return pn->num_candidates; 414 } 415 416 void try_hop(btbb_packet *pkt, btbb_piconet *pn) 417 { 418 uint8_t filter_uap = pn->UAP; 419 420 /* Decode packet - fixing clock drift in the process */ 421 btbb_decode(pkt, pn); 422 423 if (btbb_piconet_get_flag(pn, BTBB_HOP_REVERSAL_INIT)) { 424 //pn->winnowed = 0; 425 pn->pattern_indices[pn->packets_observed] = 426 pkt->clkn - pn->first_pkt_time; 427 pn->pattern_channels[pn->packets_observed] = pkt->channel; 428 pn->packets_observed++; 429 pn->total_packets_observed++; 430 btbb_winnow(pn); 431 if (btbb_piconet_get_flag(pn, BTBB_CLK27_VALID)) { 432 printf("got CLK1-27\n"); 433 printf("clock offset = %d.\n", pn->clk_offset); 434 } 435 } else { 436 if (btbb_piconet_get_flag(pn, BTBB_CLK6_VALID)) { 437 btbb_uap_from_header(pkt, pn); 438 if (btbb_piconet_get_flag(pn, BTBB_CLK27_VALID)) { 439 printf("got CLK1-27\n"); 440 printf("clock offset = %d.\n", pn->clk_offset); 441 } 442 } else { 443 if (btbb_uap_from_header(pkt, pn)) { 444 if (filter_uap == pn->UAP) { 445 printf("got CLK1-6\n"); 446 btbb_init_hop_reversal(0, pn); 447 btbb_winnow(pn); 448 } else { 449 printf("failed to confirm UAP\n"); 450 } 451 } 452 } 453 } 454 455 if(!btbb_piconet_get_flag(pn, BTBB_UAP_VALID)) { 456 btbb_piconet_set_flag(pn, BTBB_UAP_VALID, 1); 457 pn->UAP = filter_uap; 458 } 459 } 460 461 /* return the observable channel (26-50) for a given channel (0-78) */ 462 /* reset UAP/clock discovery */ 463 static void reset(btbb_piconet *pn) 464 { 465 //printf("no candidates remaining! starting over . . .\n"); 466 467 if(btbb_piconet_get_flag(pn, BTBB_HOP_REVERSAL_INIT)) { 468 free(pn->clock_candidates); 469 pn->sequence = NULL; 470 } 471 btbb_piconet_set_flag(pn, BTBB_GOT_FIRST_PACKET, 0); 472 btbb_piconet_set_flag(pn, BTBB_HOP_REVERSAL_INIT, 0); 473 btbb_piconet_set_flag(pn, BTBB_UAP_VALID, 0); 474 btbb_piconet_set_flag(pn, BTBB_CLK6_VALID, 0); 475 btbb_piconet_set_flag(pn, BTBB_CLK27_VALID, 0); 476 pn->packets_observed = 0; 477 478 /* 479 * If we have recently observed two packets in a row on the same 480 * channel, try AFH next time. If not, don't. 481 */ 482 btbb_piconet_set_flag(pn, BTBB_IS_AFH, 483 btbb_piconet_get_flag(pn, BTBB_LOOKS_LIKE_AFH)); 484 btbb_piconet_set_flag(pn, BTBB_LOOKS_LIKE_AFH, 0); 485 //int i; 486 //for(i=0; i<10; i++) 487 // pn->afh_map[i] = 0; 488 } 489 490 /* narrow a list of candidate clock values based on a single observed hop */ 491 static int channel_winnow(int offset, char channel, btbb_piconet *pn) 492 { 493 int i; 494 int new_count = 0; /* number of candidates after winnowing */ 495 char observable_channel; /* accounts for aliasing if necessary */ 496 497 /* check every candidate */ 498 for (i = 0; i < pn->num_candidates; i++) { 499 if (pn->aliased) 500 observable_channel = aliased_channel(pn->sequence[(pn->clock_candidates[i] + offset) % SEQUENCE_LENGTH]); 501 else 502 observable_channel = pn->sequence[(pn->clock_candidates[i] + offset) % SEQUENCE_LENGTH]; 503 if (observable_channel == channel) { 504 /* this candidate matches the latest hop */ 505 /* blow away old list of candidates with new one */ 506 /* safe because new_count can never be greater than i */ 507 pn->clock_candidates[new_count++] = pn->clock_candidates[i]; 508 } 509 } 510 pn->num_candidates = new_count; 511 512 if (new_count == 1) { 513 // Calculate clock offset for CLKN, not CLK1-27 514 pn->clk_offset = ((pn->clock_candidates[0]<<1) - (pn->first_pkt_time<<1)); 515 printf("\nAcquired CLK1-27 = 0x%07x\n", pn->clock_candidates[0]); 516 btbb_piconet_set_flag(pn, BTBB_CLK27_VALID, 1); 517 } 518 else if (new_count == 0) { 519 reset(pn); 520 } 521 //else { 522 //printf("%d CLK1-27 candidates remaining (channel=%d)\n", new_count, channel); 523 //} 524 525 return new_count; 526 } 527 528 /* narrow a list of candidate clock values based on all observed hops */ 529 int btbb_winnow(btbb_piconet *pn) 530 { 531 int new_count = pn->num_candidates; 532 int index, last_index; 533 uint8_t channel, last_channel; 534 535 for (; pn->winnowed < pn->packets_observed; pn->winnowed++) { 536 index = pn->pattern_indices[pn->winnowed]; 537 channel = pn->pattern_channels[pn->winnowed]; 538 new_count = channel_winnow(index, channel, pn); 539 if (new_count <= 1) 540 break; 541 542 if (pn->packets_observed > 0) { 543 last_index = pn->pattern_indices[pn->winnowed - 1]; 544 last_channel = pn->pattern_channels[pn->winnowed - 1]; 545 /* 546 * Two packets in a row on the same channel should only 547 * happen if adaptive frequency hopping is in use. 548 * There can be false positives, though, especially if 549 * there is aliasing. 550 */ 551 if (!btbb_piconet_get_flag(pn, BTBB_LOOKS_LIKE_AFH) 552 && (index == last_index + 1) 553 && (channel == last_channel)) { 554 btbb_piconet_set_flag(pn, BTBB_LOOKS_LIKE_AFH, 1); 555 printf("Hopping pattern appears to be AFH\n"); 556 } 557 } 558 } 559 560 return new_count; 561 } 562 563 /* use packet headers to determine UAP */ 564 int btbb_uap_from_header(btbb_packet *pkt, btbb_piconet *pn) 565 { 566 uint8_t UAP; 567 int count, crc_chk, first_clock = 0; 568 569 int starting = 0; 570 int remaining = 0; 571 uint32_t clkn = pkt->clkn; 572 573 if (!btbb_piconet_get_flag(pn, BTBB_GOT_FIRST_PACKET)) 574 pn->first_pkt_time = clkn; 575 576 // Set afh channel map 577 pn->afh_map[pkt->channel/8] |= 0x1 << (pkt->channel % 8); 578 579 if (pn->packets_observed < MAX_PATTERN_LENGTH) { 580 pn->pattern_indices[pn->packets_observed] = clkn - pn->first_pkt_time; 581 pn->pattern_channels[pn->packets_observed] = pkt->channel; 582 } else { 583 printf("Oops. More hops than we can remember.\n"); 584 reset(pn); 585 return 0; //FIXME ought to throw exception 586 } 587 pn->packets_observed++; 588 pn->total_packets_observed++; 589 590 /* try every possible first packet clock value */ 591 for (count = 0; count < 64; count++) { 592 /* skip eliminated candidates unless this is our first time through */ 593 if (pn->clock6_candidates[count] > -1 594 || !btbb_piconet_get_flag(pn, BTBB_GOT_FIRST_PACKET)) { 595 /* clock value for the current packet assuming count was the clock of the first packet */ 596 int clock = (count + clkn - pn->first_pkt_time) % 64; 597 starting++; 598 UAP = try_clock(clock, pkt); 599 crc_chk = -1; 600 601 /* if this is the first packet: populate the candidate list */ 602 /* if not: check CRCs if UAPs match */ 603 if (!btbb_piconet_get_flag(pn, BTBB_GOT_FIRST_PACKET) 604 || UAP == pn->clock6_candidates[count]) 605 crc_chk = crc_check(clock, pkt); 606 607 if (btbb_piconet_get_flag(pn, BTBB_UAP_VALID) && 608 (UAP != pn->UAP)) 609 crc_chk = -1; 610 611 switch(crc_chk) { 612 case -1: /* UAP mismatch */ 613 case 0: /* CRC failure */ 614 pn->clock6_candidates[count] = -1; 615 break; 616 617 case 1: /* inconclusive result */ 618 case 2: /* Inconclusive, but looks better */ 619 pn->clock6_candidates[count] = UAP; 620 /* remember this count because it may be the correct clock of the first packet */ 621 first_clock = count; 622 remaining++; 623 break; 624 625 default: /* CRC success */ 626 pn->clk_offset = (count - (pn->first_pkt_time & 0x3f)) & 0x3f; 627 if (!btbb_piconet_get_flag(pn, BTBB_UAP_VALID)) 628 printf("Correct CRC! UAP = 0x%x found after %d total packets.\n", 629 UAP, pn->total_packets_observed); 630 else 631 printf("Correct CRC! CLK6 = 0x%x found after %d total packets.\n", 632 pn->clk_offset, pn->total_packets_observed); 633 pn->UAP = UAP; 634 btbb_piconet_set_flag(pn, BTBB_CLK6_VALID, 1); 635 btbb_piconet_set_flag(pn, BTBB_UAP_VALID, 1); 636 pn->total_packets_observed = 0; 637 return 1; 638 } 639 } 640 } 641 642 btbb_piconet_set_flag(pn, BTBB_GOT_FIRST_PACKET, 1); 643 644 //printf("reduced from %d to %d CLK1-6 candidates\n", starting, remaining); 645 646 if (remaining == 1) { 647 pn->clk_offset = (first_clock - (pn->first_pkt_time & 0x3f)) & 0x3f; 648 if (!btbb_piconet_get_flag(pn, BTBB_UAP_VALID)) 649 printf("We have a winner! UAP = 0x%x found after %d total packets.\n", 650 pn->clock6_candidates[first_clock], pn->total_packets_observed); 651 else 652 printf("We have a winner! CLK6 = 0x%x found after %d total packets.\n", 653 pn->clk_offset, pn->total_packets_observed); 654 pn->UAP = pn->clock6_candidates[first_clock]; 655 btbb_piconet_set_flag(pn, BTBB_CLK6_VALID, 1); 656 btbb_piconet_set_flag(pn, BTBB_UAP_VALID, 1); 657 pn->total_packets_observed = 0; 658 return 1; 659 } 660 661 if (remaining == 0) { 662 reset(pn); 663 } 664 665 return 0; 666 } 667 668 /* add a packet to the queue */ 669 static void enqueue(btbb_packet *pkt, btbb_piconet *pn) 670 { 671 pkt_queue *head; 672 //pkt_queue item; 673 674 btbb_packet_ref(pkt); 675 pkt_queue item = {pkt, NULL}; 676 head = pn->queue; 677 678 if (head == NULL) { 679 pn->queue = &item; 680 } else { 681 for(; head->next != NULL; head = head->next) 682 ; 683 head->next = &item; 684 } 685 } 686 687 /* pull the first packet from the queue (FIFO) */ 688 static btbb_packet *dequeue(btbb_piconet *pn) 689 { 690 btbb_packet *pkt; 691 692 if (pn->queue == NULL) { 693 pkt = NULL; 694 } else { 695 pkt = pn->queue->pkt; 696 pn->queue = pn->queue->next; 697 btbb_packet_unref(pkt); 698 } 699 700 return pkt; 701 } 702 703 /* decode the whole packet */ 704 int btbb_decode(btbb_packet* pkt, btbb_piconet *pn) 705 { 706 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 0); 707 uint8_t clk6, i, best_clk; 708 int rv = 0, max_rv = 0; 709 if (btbb_piconet_get_flag(pn, BTBB_CLK27_VALID)) { 710 /* Removing this section until we can more reliably handle AFH */ 711 //if(pn->sequence == NULL) 712 // get_hop_pattern(pn); 713 //clk6 = pkt->clock & 0x3f; 714 //for(i=0; i<64; i++) { 715 // pkt->clock = (pkt->clock & 0xffffffc0) | ((clk6 + i) & 0x3f); 716 // if ((pn->sequence[pkt->clock] == pkt->channel) && (btbb_decode_header(pkt))) { 717 // rv = btbb_decode_payload(pkt); 718 // if(rv > max_rv) { 719 // max_rv = rv; 720 // best_clk = (clk6 + i) & 0x3f; 721 // } 722 // } 723 //} 724 725 // If we found nothing, try again, ignoring channel 726 if(max_rv <= 1) { 727 clk6 = pkt->clock & 0x3f; 728 for(i=0; i<64; i++) { 729 pkt->clock = (pkt->clock & 0xffffffc0) | ((clk6 + i) & 0x3f); 730 if (btbb_decode_header(pkt)) { 731 rv = btbb_decode_payload(pkt); 732 if(rv > max_rv) { 733 //printf("Packet decoded with clock 0x%07x (rv=%d)\n", pkt->clock, rv); 734 //btbb_print_packet(pkt); 735 max_rv = rv; 736 best_clk = (clk6 + i) & 0x3f; 737 } 738 } 739 } 740 } 741 } else 742 if (btbb_decode_header(pkt)) { 743 for(i=0; i<64; i++) { 744 pkt->clock = (pkt->clock & 0xffffffc0) | (i & 0x3f); 745 if (btbb_decode_header(pkt)) { 746 rv = btbb_decode_payload(pkt); 747 if(rv > max_rv) { 748 //printf("Packet decoded with clock 0x%02x (rv=%d)\n", i, rv); 749 //btbb_print_packet(pkt); 750 max_rv = rv; 751 best_clk = i & 0x3f; 752 } 753 } 754 } 755 } 756 /* If we were successful, print the packet */ 757 if(max_rv > 0) { 758 pkt->clock = (pkt->clock & 0xffffffc0) | (best_clk & 0x3f); 759 btbb_decode_payload(pkt); 760 printf("Packet decoded with clock 0x%02x (rv=%d)\n", i, rv); 761 btbb_print_packet(pkt); 762 } 763 764 return max_rv; 765 } 766 767 /* Print AFH map from observed packets */ 768 void btbb_print_afh_map(btbb_piconet *pn) { 769 uint8_t *afh_map; 770 afh_map = pn->afh_map; 771 772 /* Printed ch78 -> ch0 */ 773 printf("\tAFH Map=0x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n", 774 afh_map[9], afh_map[8], afh_map[7], afh_map[6], afh_map[5], 775 afh_map[4], afh_map[3], afh_map[2], afh_map[1], afh_map[0]); 776 } 777 778 /* Container for survey piconets */ 779 typedef struct { 780 uint32_t key; /* LAP */ 781 btbb_piconet *pn; 782 UT_hash_handle hh; 783 } survey_hash; 784 785 static survey_hash *piconet_survey = NULL; 786 787 /* A bit of a hack? to set survey mode */ 788 static int survey_mode = 0; 789 int btbb_init_survey() { 790 survey_mode = 1; 791 return 0; 792 } 793 794 /* Check for existing piconets in survey results */ 795 btbb_piconet *get_piconet(uint32_t lap) 796 { 797 survey_hash *s; 798 btbb_piconet *pn; 799 HASH_FIND(hh, piconet_survey, &lap, 4, s); 800 801 if (s == NULL) { 802 pn = btbb_piconet_new(); 803 btbb_init_piconet(pn, lap); 804 805 s = malloc(sizeof(survey_hash)); 806 s->key = lap; 807 s->pn = pn; 808 HASH_ADD(hh, piconet_survey, key, 4, s); 809 } else { 810 pn = s->pn; 811 } 812 return pn; 813 } 814 815 /* Destructively iterate over survey results */ 816 btbb_piconet *btbb_next_survey_result() { 817 btbb_piconet *pn = NULL; 818 survey_hash *tmp; 819 820 if (piconet_survey != NULL) { 821 pn = piconet_survey->pn; 822 tmp = piconet_survey; 823 piconet_survey = piconet_survey->hh.next; 824 free(tmp); 825 } 826 return pn; 827 } 828 829 int btbb_process_packet(btbb_packet *pkt, btbb_piconet *pn) { 830 if (survey_mode) { 831 pn = get_piconet(btbb_packet_get_lap(pkt)); 832 btbb_piconet_set_channel_seen(pn, pkt->channel); 833 if(btbb_header_present(pkt) && !btbb_piconet_get_flag(pn, BTBB_UAP_VALID)) 834 btbb_uap_from_header(pkt, pn); 835 return 0; 836 } 837 /* If piconet structure is given, a LAP is given, and packet 838 * header is readable, do further analysis. If UAP has not yet 839 * been determined, attempt to calculate it from headers. Once 840 * UAP is known, try to determine clk6 and clk27. Once clocks 841 * are known, follow the piconet. */ 842 if (pn && btbb_piconet_get_flag(pn, BTBB_LAP_VALID) && 843 btbb_header_present(pkt)) { 844 845 /* Have LAP/UAP/clocks, now hopping along with the piconet. */ 846 if (btbb_piconet_get_flag(pn, BTBB_FOLLOWING)) { 847 btbb_packet_set_uap(pkt, btbb_piconet_get_uap(pn)); 848 btbb_packet_set_flag(pkt, BTBB_CLK6_VALID, 1); 849 btbb_packet_set_flag(pkt, BTBB_CLK27_VALID, 1); 850 851 if(btbb_decode(pkt, pn)) 852 btbb_print_packet(pkt); 853 else 854 printf("Failed to decode packet\n"); 855 } 856 857 /* Have LAP/UAP, need clocks. */ 858 else if (btbb_piconet_get_uap(pn)) { 859 try_hop(pkt, pn); 860 if (btbb_piconet_get_flag(pn, BTBB_CLK6_VALID) && 861 btbb_piconet_get_flag(pn, BTBB_CLK27_VALID)) { 862 btbb_piconet_set_flag(pn, BTBB_FOLLOWING, 1); 863 return -1; 864 } 865 } 866 867 /* Have LAP, need UAP. */ 868 else { 869 btbb_uap_from_header(pkt, pn); 870 } 871 } 872 return 0; 873 } 874