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