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