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