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