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