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 */
count_bits(uint8_t n)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 *
btbb_piconet_new(void)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
btbb_piconet_ref(btbb_piconet * pn)50 btbb_piconet_ref(btbb_piconet *pn)
51 {
52 pn->refcount++;
53 }
54
55 void
btbb_piconet_unref(btbb_piconet * pn)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;
btbb_init_survey()65 int btbb_init_survey() {
66 survey_mode = 1;
67 return 0;
68 }
69
btbb_init_piconet(btbb_piconet * pn,uint32_t lap)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
btbb_piconet_set_flag(btbb_piconet * pn,int flag,int val)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
btbb_piconet_get_flag(const btbb_piconet * pn,const int flag)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
btbb_piconet_set_uap(btbb_piconet * pn,uint8_t uap)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
btbb_piconet_get_uap(const btbb_piconet * pn)96 uint8_t btbb_piconet_get_uap(const btbb_piconet *pn)
97 {
98 return pn->UAP;
99 }
100
btbb_piconet_get_lap(const btbb_piconet * pn)101 uint32_t btbb_piconet_get_lap(const btbb_piconet *pn)
102 {
103 return pn->LAP;
104 }
105
btbb_piconet_get_nap(const btbb_piconet * pn)106 uint16_t btbb_piconet_get_nap(const btbb_piconet *pn)
107 {
108 return pn->NAP;
109 }
110
btbb_piconet_get_bdaddr(const btbb_piconet * pn)111 uint64_t btbb_piconet_get_bdaddr(const btbb_piconet *pn)
112 {
113 return ((uint64_t) pn->NAP) << 32 | ((uint32_t) pn->UAP) << 24 | pn->LAP;
114 }
115
btbb_piconet_get_clk_offset(const btbb_piconet * pn)116 int btbb_piconet_get_clk_offset(const btbb_piconet *pn)
117 {
118 return pn->clk_offset;
119 }
120
btbb_piconet_set_clk_offset(btbb_piconet * pn,int clk_offset)121 void btbb_piconet_set_clk_offset(btbb_piconet *pn, int clk_offset)
122 {
123 pn->clk_offset = clk_offset;
124 }
125
btbb_piconet_set_afh_map(btbb_piconet * pn,uint8_t * afh_map)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
btbb_piconet_get_afh_map(btbb_piconet * pn)138 uint8_t *btbb_piconet_get_afh_map(btbb_piconet *pn) {
139 return pn->afh_map;
140 }
141
btbb_piconet_set_channel_seen(btbb_piconet * pn,uint8_t channel)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
btbb_piconet_clear_channel_seen(btbb_piconet * pn,uint8_t channel)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
btbb_piconet_get_channel_seen(btbb_piconet * pn,uint8_t channel)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 */
precalc(btbb_piconet * pn)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 */
address_precalc(int address,btbb_piconet * pn)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
perm5(int z,int p_high,int p_low)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 */
perm5(int z,int p_high,int p_low)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
perm_table_init(void)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 */
fast_perm(int z,int p_high,int p_low)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 */
gen_hops(btbb_piconet * pn)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 */
gen_hop_pattern(btbb_piconet * pn)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 */
get_hop_pattern(btbb_piconet * pn)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) | ((uint32_t)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 */
single_hop(int clock,btbb_piconet * pn)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 */
hop(int clock,btbb_piconet * pn)444 char hop(int clock, btbb_piconet *pn)
445 {
446 return pn->sequence[clock];
447 }
448
aliased_channel(char channel)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) */
init_candidates(char channel,int known_clock_bits,btbb_piconet * pn)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 */
btbb_init_hop_reversal(int aliased,btbb_piconet * pn)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
try_hop(btbb_packet * pkt,btbb_piconet * pn)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);
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 */
reset(btbb_piconet * pn)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 */
channel_winnow(int offset,char channel,btbb_piconet * pn)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 */
btbb_winnow(btbb_piconet * pn)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 */
btbb_uap_from_header(btbb_packet * pkt,btbb_piconet * pn)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 /* Print AFH map from observed packets */
btbb_print_afh_map(btbb_piconet * pn)792 void btbb_print_afh_map(btbb_piconet *pn) {
793 uint8_t *afh_map;
794 afh_map = pn->afh_map;
795
796 /* Print like hcitool does */
797 printf("AFH map: 0x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
798 afh_map[0], afh_map[1], afh_map[2], afh_map[3], afh_map[4],
799 afh_map[5], afh_map[6], afh_map[7], afh_map[8], afh_map[9]);
800
801 // /* Printed ch78 -> ch0 */
802 // printf("\tAFH Map=0x%02x%02x%02x%02x%02x%02x%02x%02x%02x%02x\n",
803 // afh_map[9], afh_map[8], afh_map[7], afh_map[6], afh_map[5],
804 // afh_map[4], afh_map[3], afh_map[2], afh_map[1], afh_map[0]);
805 }
806
807 /* Container for survey piconets */
808 typedef struct {
809 uint32_t key; /* LAP */
810 btbb_piconet *pn;
811 UT_hash_handle hh;
812 } survey_hash;
813
814 static survey_hash *piconet_survey = NULL;
815
816 /* Check for existing piconets in survey results */
get_piconet(uint32_t lap)817 btbb_piconet *get_piconet(uint32_t lap)
818 {
819 survey_hash *s;
820 btbb_piconet *pn;
821 HASH_FIND(hh, piconet_survey, &lap, 4, s);
822
823 if (s == NULL) {
824 pn = btbb_piconet_new();
825 btbb_init_piconet(pn, lap);
826
827 s = malloc(sizeof(survey_hash));
828 s->key = lap;
829 s->pn = pn;
830 HASH_ADD(hh, piconet_survey, key, 4, s);
831 } else {
832 pn = s->pn;
833 }
834 return pn;
835 }
836
837 /* Destructively iterate over survey results */
btbb_next_survey_result()838 btbb_piconet *btbb_next_survey_result() {
839 btbb_piconet *pn = NULL;
840 survey_hash *tmp;
841
842 if (piconet_survey != NULL) {
843 pn = piconet_survey->pn;
844 tmp = piconet_survey;
845 piconet_survey = piconet_survey->hh.next;
846 free(tmp);
847 }
848 return pn;
849 }
850
btbb_process_packet(btbb_packet * pkt,btbb_piconet * pn)851 int btbb_process_packet(btbb_packet *pkt, btbb_piconet *pn) {
852 if (survey_mode) {
853 pn = get_piconet(btbb_packet_get_lap(pkt));
854 btbb_piconet_set_channel_seen(pn, pkt->channel);
855 if(btbb_header_present(pkt) && !btbb_piconet_get_flag(pn, BTBB_UAP_VALID))
856 btbb_uap_from_header(pkt, pn);
857 return 0;
858 }
859
860 if(pn)
861 btbb_piconet_set_channel_seen(pn, pkt->channel);
862
863 /* If piconet structure is given, a LAP is given, and packet
864 * header is readable, do further analysis. If UAP has not yet
865 * been determined, attempt to calculate it from headers. Once
866 * UAP is known, try to determine clk6 and clk27. Once clocks
867 * are known, follow the piconet. */
868 if (pn && btbb_piconet_get_flag(pn, BTBB_LAP_VALID) &&
869 btbb_header_present(pkt)) {
870
871 /* Have LAP/UAP/clocks, now hopping along with the piconet. */
872 if (btbb_piconet_get_flag(pn, BTBB_FOLLOWING)) {
873 btbb_packet_set_uap(pkt, btbb_piconet_get_uap(pn));
874 btbb_packet_set_flag(pkt, BTBB_CLK6_VALID, 1);
875 btbb_packet_set_flag(pkt, BTBB_CLK27_VALID, 1);
876
877 if(btbb_decode(pkt))
878 btbb_print_packet(pkt);
879 else
880 printf("Failed to decode packet\n");
881 }
882
883 /* Have LAP/UAP, need clocks. */
884 else if (btbb_piconet_get_uap(pn)) {
885 try_hop(pkt, pn);
886 if (btbb_piconet_get_flag(pn, BTBB_CLK6_VALID) &&
887 btbb_piconet_get_flag(pn, BTBB_CLK27_VALID)) {
888 btbb_piconet_set_flag(pn, BTBB_FOLLOWING, 1);
889 return -1;
890 }
891 }
892
893 /* Have LAP, need UAP. */
894 else {
895 btbb_uap_from_header(pkt, pn);
896 }
897 }
898 return 0;
899 }
900