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 <stdio.h>
24 #include <stdlib.h>
25
26 #include "bluetooth_packet.h"
27 #include "uthash.h"
28 #include "sw_check_tables.h"
29
30 #define MIN(X, Y) (((X) < (Y)) ? (X) : (Y))
31
32 /* Maximum number of AC errors supported by library. Caller may
33 * specify any value <= AC_ERROR_LIMIT in btbb_init(). */
34 #define AC_ERROR_LIMIT 5
35
36 /* maximum number of bit errors for known syncwords */
37 #define MAX_SYNCWORD_ERRS 5
38
39 /* maximum number of bit errors in */
40 #define MAX_BARKER_ERRORS 1
41
42 /* default codeword modified for PN sequence and barker code */
43 #define DEFAULT_CODEWORD 0xb0000002c7820e7eULL
44
45 /* Default access code, used for calculating syndromes */
46 #define DEFAULT_AC 0xcc7b7268ff614e1bULL
47
48 /* index into whitening data array */
49 static const uint8_t INDICES[] = {99, 85, 17, 50, 102, 58, 108, 45, 92, 62, 32, 118, 88, 11, 80, 2, 37, 69, 55, 8, 20, 40, 74, 114, 15, 106, 30, 78, 53, 72, 28, 26, 68, 7, 39, 113, 105, 77, 71, 25, 84, 49, 57, 44, 61, 117, 10, 1, 123, 124, 22, 125, 111, 23, 42, 126, 6, 112, 76, 24, 48, 43, 116, 0};
50
51 /* whitening data */
52 static const uint8_t WHITENING_DATA[] = {1, 1, 1, 0, 0, 0, 1, 1, 1, 0, 1, 1, 0, 0, 0, 1, 0, 1, 0, 0, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 1, 1, 0, 1, 1, 1, 1, 0, 0, 1, 1, 1, 0, 0, 1, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1};
53
54 /* lookup table for barker code hamming distance */
55 static const uint8_t BARKER_DISTANCE[] = {
56 3,3,3,2,3,2,2,1,2,3,3,3,3,3,3,2,2,3,3,3,3,3,3,2,1,2,2,3,2,3,3,3,
57 3,2,2,1,2,1,1,0,3,3,3,2,3,2,2,1,3,3,3,2,3,2,2,1,2,3,3,3,3,3,3,2,
58 2,3,3,3,3,3,3,2,1,2,2,3,2,3,3,3,1,2,2,3,2,3,3,3,0,1,1,2,1,2,2,3,
59 3,3,3,2,3,2,2,1,2,3,3,3,3,3,3,2,2,3,3,3,3,3,3,2,1,2,2,3,2,3,3,3};
60
61 /* string representations of packet type */
62 static const char * const TYPE_NAMES[] = {
63 "NULL", "POLL", "FHS", "DM1", "DH1/2-DH1", "HV1", "HV2/2-EV3", "HV3/EV3/3-EV3",
64 "DV/3-DH1", "AUX1", "DM3/2-DH3", "DH3/3-DH3", "EV4/2-EV5", "EV5/3-EV5", "DM5/2-DH5", "DH5/3-DH5"
65 };
66
67 /*
68 * generator matrix for sync word (64,30) linear block code
69 * based on polynomial 0260534236651
70 * thanks to http://www.ee.unb.ca/cgi-bin/tervo/polygen.pl
71 * modified for barker code
72 */
73 static const uint64_t sw_matrix[] = {
74 0xfe000002a0d1c014ULL, 0x01000003f0b9201fULL, 0x008000033ae40edbULL, 0x004000035fca99b9ULL,
75 0x002000036d5dd208ULL, 0x00100001b6aee904ULL, 0x00080000db577482ULL, 0x000400006dabba41ULL,
76 0x00020002f46d43f4ULL, 0x000100017a36a1faULL, 0x00008000bd1b50fdULL, 0x000040029c3536aaULL,
77 0x000020014e1a9b55ULL, 0x0000100265b5d37eULL, 0x0000080132dae9bfULL, 0x000004025bd5ea0bULL,
78 0x00000203ef526bd1ULL, 0x000001033511ab3cULL, 0x000000819a88d59eULL, 0x00000040cd446acfULL,
79 0x00000022a41aabb3ULL, 0x0000001390b5cb0dULL, 0x0000000b0ae27b52ULL, 0x0000000585713da9ULL};
80
81 static const uint64_t barker_correct[] = {
82 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
83 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
84 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL,
85 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
86 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL,
87 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
88 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL,
89 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL,
90 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
91 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
92 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
93 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
94 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
95 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
96 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL,
97 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
98 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL,
99 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
100 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL,
101 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL,
102 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL,
103 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL,
104 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL,
105 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL,
106 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
107 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
108 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL,
109 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
110 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL,
111 0xb000000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL, 0x4e00000000000000ULL,
112 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL,
113 0xb000000000000000ULL, 0xb000000000000000ULL, 0xb000000000000000ULL, 0x4e00000000000000ULL};
114
115 static const uint64_t pn = 0x83848D96BBCC54FCULL;
116
117 static const uint16_t fec23_gen_matrix[] = {
118 0x2c01, 0x5802, 0x1c04, 0x3808, 0x7010,
119 0x4c20, 0x3440, 0x6880, 0x7d00, 0x5600};
120
121 typedef struct {
122 uint64_t syndrome; /* key */
123 uint64_t error;
124 UT_hash_handle hh;
125 } syndrome_struct;
126
127 static syndrome_struct *syndrome_map = NULL;
128
add_syndrome(uint64_t syndrome,uint64_t error)129 static void add_syndrome(uint64_t syndrome, uint64_t error)
130 {
131 syndrome_struct *s;
132 s = malloc(sizeof(syndrome_struct));
133 s->syndrome = syndrome;
134 s->error = error;
135
136 HASH_ADD(hh, syndrome_map, syndrome, 8, s);
137 }
138
find_syndrome(uint64_t syndrome)139 static syndrome_struct *find_syndrome(uint64_t syndrome)
140 {
141 syndrome_struct *s;
142
143 HASH_FIND(hh, syndrome_map, &syndrome, 8, s);
144 return s;
145 }
146
gen_syndrome(uint64_t codeword)147 static uint64_t gen_syndrome(uint64_t codeword)
148 {
149 uint64_t syndrome = codeword & 0xffffffff;
150 codeword >>= 32;
151 syndrome ^= sw_check_table4[codeword & 0xff];
152 codeword >>= 8;
153 syndrome ^= sw_check_table5[codeword & 0xff];
154 codeword >>= 8;
155 syndrome ^= sw_check_table6[codeword & 0xff];
156 codeword >>= 8;
157 syndrome ^= sw_check_table7[codeword & 0xff];
158 return syndrome;
159 }
160
cycle(uint64_t error,int start,int depth,uint64_t codeword)161 static void cycle(uint64_t error, int start, int depth, uint64_t codeword)
162 {
163 uint64_t new_error, syndrome, base;
164 int i;
165 base = 1;
166 depth -= 1;
167 for (i = start; i < 58; i++)
168 {
169 new_error = (base << i);
170 new_error |= error;
171 if (depth)
172 cycle(new_error, i + 1, depth, codeword);
173 else {
174 syndrome = gen_syndrome(codeword ^ new_error);
175 add_syndrome(syndrome, new_error);
176 }
177 }
178 }
179
gen_syndrome_map(int bit_errors)180 static void gen_syndrome_map(int bit_errors)
181 {
182 int i;
183 for(i = 1; i <= bit_errors; i++)
184 cycle(0, 0, i, DEFAULT_AC);
185 }
186
187 /* Generate Sync Word from an LAP */
btbb_gen_syncword(const int LAP)188 uint64_t btbb_gen_syncword(const int LAP)
189 {
190 int i;
191 uint64_t codeword = DEFAULT_CODEWORD;
192
193 /* the sync word generated is in host order, not air order */
194 for (i = 0; i < 24; i++)
195 if (LAP & (0x800000 >> i))
196 codeword ^= sw_matrix[i];
197
198 return codeword;
199 }
200
init_packet(btbb_packet * pkt,uint32_t lap,uint8_t ac_errors)201 static void init_packet(btbb_packet *pkt, uint32_t lap, uint8_t ac_errors)
202 {
203 pkt->LAP = lap;
204 pkt->ac_errors = ac_errors;
205
206 pkt->flags = 0;
207 btbb_packet_set_flag(pkt, BTBB_WHITENED, 1);
208 }
209
210 /* Convert some number of bits of an air order array to a host order integer */
air_to_host8(const char * air_order,const int bits)211 static uint8_t air_to_host8(const char *air_order, const int bits)
212 {
213 int i;
214 uint8_t host_order = 0;
215 for (i = 0; i < bits; i++)
216 host_order |= ((uint8_t)air_order[i] << i);
217 return host_order;
218 }
air_to_host16(const char * air_order,const int bits)219 static uint16_t air_to_host16(const char *air_order, const int bits)
220 {
221 int i;
222 uint16_t host_order = 0;
223 for (i = 0; i < bits; i++)
224 host_order |= ((uint16_t)air_order[i] << i);
225 return host_order;
226 }
air_to_host32(const char * air_order,const int bits)227 static uint32_t air_to_host32(const char *air_order, const int bits)
228 {
229 int i;
230 uint32_t host_order = 0;
231 for (i = 0; i < bits; i++)
232 host_order |= ((uint32_t)air_order[i] << i);
233 return host_order;
234 }
air_to_host64(const char * air_order,const int bits)235 static uint64_t air_to_host64(const char *air_order, const int bits)
236 {
237 int i;
238 uint64_t host_order = 0;
239 for (i = 0; i < bits; i++)
240 host_order |= ((uint64_t)air_order[i] << i);
241 return host_order;
242 }
243
244 ///* Convert some number of bits in a host order integer to an air order array */
245 //static void host_to_air(const uint8_t host_order, char *air_order, const int bits)
246 //{
247 // int i;
248 // for (i = 0; i < bits; i++)
249 // air_order[i] = (host_order >> i) & 0x01;
250 //}
251
252 /* count the number of 1 bits in a uint64_t */
count_bits(uint64_t n)253 static uint8_t count_bits(uint64_t n)
254 {
255 #ifdef __GNUC__
256 return (uint8_t) __builtin_popcountll (n);
257 #else
258 uint8_t i = 0;
259 for (i = 0; n != 0; i++)
260 n &= n - 1;
261 return i;
262 #endif
263 }
264
265 #ifndef RELEASE
266 #define RELEASE "unknown"
267 #endif
btbb_get_release(void)268 const char* btbb_get_release(void) {
269 return RELEASE;
270 }
271
272 #ifndef VERSION
273 #define VERSION "unknown"
274 #endif
btbb_get_version(void)275 const char* btbb_get_version(void) {
276 return VERSION;
277 }
278
btbb_init(int max_ac_errors)279 int btbb_init(int max_ac_errors)
280 {
281 /* Sanity check max_ac_errors. */
282 if ( (max_ac_errors < 0) || (max_ac_errors > AC_ERROR_LIMIT) ) {
283 fprintf(stderr, "%s: max_ac_errors out of range\n",
284 __FUNCTION__);
285 return -1;
286 }
287
288 if ((syndrome_map == NULL) && (max_ac_errors))
289 gen_syndrome_map(max_ac_errors);
290
291 return 0;
292 }
293
294 btbb_packet *
btbb_packet_new(void)295 btbb_packet_new(void)
296 {
297 btbb_packet *pkt = (btbb_packet *)calloc(1, sizeof(btbb_packet));
298 if(pkt)
299 pkt->refcount = 1;
300 else
301 fprintf(stderr, "Unable to allocate packet");
302 return pkt;
303 }
304
305 void
btbb_packet_ref(btbb_packet * pkt)306 btbb_packet_ref(btbb_packet *pkt)
307 {
308 pkt->refcount++;
309 }
310
311 void
btbb_packet_unref(btbb_packet * pkt)312 btbb_packet_unref(btbb_packet *pkt)
313 {
314 pkt->refcount--;
315 if (pkt->refcount == 0)
316 free(pkt);
317 }
318
btbb_packet_get_lap(const btbb_packet * pkt)319 uint32_t btbb_packet_get_lap(const btbb_packet *pkt)
320 {
321 return pkt->LAP;
322 }
323
btbb_packet_set_uap(btbb_packet * pkt,uint8_t uap)324 void btbb_packet_set_uap(btbb_packet *pkt, uint8_t uap)
325 {
326 pkt->UAP = uap;
327 btbb_packet_set_flag(pkt, BTBB_UAP_VALID, 1);
328 }
329
btbb_packet_get_uap(const btbb_packet * pkt)330 uint8_t btbb_packet_get_uap(const btbb_packet *pkt)
331 {
332 return pkt->UAP;
333 }
334
btbb_packet_get_nap(const btbb_packet * pkt)335 uint16_t btbb_packet_get_nap(const btbb_packet *pkt)
336 {
337 return pkt->NAP;
338 }
339
btbb_packet_get_clkn(const btbb_packet * pkt)340 uint32_t btbb_packet_get_clkn(const btbb_packet *pkt) {
341 return pkt->clkn;
342 }
343
btbb_packet_get_channel(const btbb_packet * pkt)344 uint8_t btbb_packet_get_channel(const btbb_packet *pkt) {
345 return pkt->channel;
346 }
347
btbb_packet_set_modulation(btbb_packet * pkt,uint8_t modulation)348 void btbb_packet_set_modulation(btbb_packet *pkt, uint8_t modulation) {
349 pkt->modulation = modulation;
350 }
351
btbb_packet_get_modulation(const btbb_packet * pkt)352 uint8_t btbb_packet_get_modulation(const btbb_packet *pkt) {
353 return pkt->modulation;
354 }
355
btbb_packet_set_transport(btbb_packet * pkt,uint8_t transport)356 void btbb_packet_set_transport(btbb_packet *pkt, uint8_t transport) {
357 pkt->transport = transport;
358 }
359
btbb_packet_get_transport(const btbb_packet * pkt)360 uint8_t btbb_packet_get_transport(const btbb_packet *pkt) {
361 return pkt->transport;
362 }
363
btbb_packet_get_ac_errors(const btbb_packet * pkt)364 uint8_t btbb_packet_get_ac_errors(const btbb_packet *pkt) {
365 return pkt->ac_errors;
366 }
367
promiscuous_packet_search(char * stream,int search_length,uint32_t * lap,int max_ac_errors,uint8_t * ac_errors)368 int promiscuous_packet_search(char *stream, int search_length, uint32_t *lap,
369 int max_ac_errors, uint8_t *ac_errors) {
370 uint64_t syncword, codeword, syndrome, corrected_barker;
371 syndrome_struct *errors;
372 char *symbols;
373 int count, offset = -1;
374
375 /* Barker code at end of sync word (includes
376 * MSB of LAP) is used as a rough filter.
377 */
378 uint8_t barker = air_to_host8(&stream[57], 6);
379 barker <<= 1;
380
381 for (count = 0; count < search_length; count++) {
382 symbols = &stream[count];
383 barker >>= 1;
384 barker |= (symbols[63] << 6);
385 if (BARKER_DISTANCE[barker] <= MAX_BARKER_ERRORS) {
386 // Error correction
387 syncword = air_to_host64(symbols, 64);
388
389 /* correct the barker code with a simple comparison */
390 corrected_barker = barker_correct[(uint8_t)(syncword >> 57)];
391 syncword = (syncword & 0x01ffffffffffffffULL) | corrected_barker;
392
393 codeword = syncword ^ pn;
394
395 /* Zero syndrome -> good codeword. */
396 syndrome = gen_syndrome(codeword);
397 *ac_errors = 0;
398
399 /* Try to fix errors in bad codeword. */
400 if (syndrome) {
401 errors = find_syndrome(syndrome);
402 if (errors != NULL) {
403 syncword ^= errors->error;
404 *ac_errors = count_bits(errors->error);
405 syndrome = 0;
406 }
407 else {
408 *ac_errors = 0xff; // fail
409 }
410 }
411
412 if (*ac_errors <= max_ac_errors) {
413 *lap = (syncword >> 34) & 0xffffff;
414 offset = count;
415 break;
416 }
417 }
418 }
419 return offset;
420 }
421
422 /* Matching a specific LAP */
find_known_lap(char * stream,int search_length,uint32_t lap,int max_ac_errors,uint8_t * ac_errors)423 int find_known_lap(char *stream, int search_length, uint32_t lap,
424 int max_ac_errors, uint8_t *ac_errors) {
425 uint64_t syncword, ac;
426 char *symbols;
427 int count, offset = -1;
428
429 ac = btbb_gen_syncword(lap);
430 for (count = 0; count < search_length; count++) {
431 symbols = &stream[count];
432 syncword = air_to_host64(symbols, 64);
433 *ac_errors = count_bits(syncword ^ ac);
434
435 if (*ac_errors <= max_ac_errors) {
436 offset = count;
437 break;
438 }
439 }
440 return offset;
441 }
442
443 /* Looks for an AC in the stream */
btbb_find_ac(char * stream,int search_length,uint32_t lap,int max_ac_errors,btbb_packet ** pkt_ptr)444 int btbb_find_ac(char *stream, int search_length, uint32_t lap,
445 int max_ac_errors, btbb_packet **pkt_ptr) {
446 int offset;
447 uint8_t ac_errors;
448
449 /* Matching any LAP */
450 if (lap == LAP_ANY)
451 offset = promiscuous_packet_search(stream, search_length, &lap,
452 max_ac_errors, &ac_errors);
453 else
454 offset = find_known_lap(stream, search_length, lap,
455 max_ac_errors, &ac_errors);
456
457 if (offset >= 0) {
458 if (*pkt_ptr == NULL)
459 *pkt_ptr = btbb_packet_new();
460 init_packet(*pkt_ptr, lap, ac_errors);
461 }
462
463 return offset;
464 }
465
466 /* Copy data (symbols) into packet and set rx data. */
btbb_packet_set_data(btbb_packet * pkt,char * data,int length,uint8_t channel,uint32_t clkn)467 void btbb_packet_set_data(btbb_packet *pkt, char *data, int length,
468 uint8_t channel, uint32_t clkn)
469 {
470 int i;
471
472 if (length > MAX_SYMBOLS)
473 length = MAX_SYMBOLS;
474 for (i = 0; i < length; i++)
475 pkt->symbols[i] = data[i];
476
477 pkt->length = length;
478 pkt->channel = channel;
479 pkt->clkn = clkn >> 1; // really CLK1
480 }
481
btbb_packet_set_flag(btbb_packet * pkt,int flag,int val)482 void btbb_packet_set_flag(btbb_packet *pkt, int flag, int val)
483 {
484 uint32_t mask = 1L << flag;
485 pkt->flags &= ~mask;
486 if (val)
487 pkt->flags |= mask;
488 }
489
btbb_packet_get_flag(const btbb_packet * pkt,int flag)490 int btbb_packet_get_flag(const btbb_packet *pkt, int flag)
491 {
492 uint32_t mask = 1L << flag;
493 return ((pkt->flags & mask) != 0);
494 }
495
btbb_get_symbols(const btbb_packet * pkt)496 const char *btbb_get_symbols(const btbb_packet* pkt)
497 {
498 return (const char*) pkt->symbols;
499 }
500
btbb_packet_get_payload_length(const btbb_packet * pkt)501 int btbb_packet_get_payload_length(const btbb_packet* pkt)
502 {
503 return pkt->payload_length;
504 }
505
btbb_get_payload(const btbb_packet * pkt)506 const char *btbb_get_payload(const btbb_packet* pkt)
507 {
508 return (const char*) pkt->payload;
509 }
510
btbb_get_payload_packed(const btbb_packet * pkt,char * dst)511 int btbb_get_payload_packed(const btbb_packet* pkt, char *dst)
512 {
513 int i;
514 for(i=0;i<pkt->payload_length;i++)
515 dst[i] = (char) air_to_host8(&pkt->payload[i*8], 8);
516 return pkt->payload_length;
517 }
518
btbb_packet_get_type(const btbb_packet * pkt)519 uint8_t btbb_packet_get_type(const btbb_packet* pkt)
520 {
521 return pkt->packet_type;
522 }
523
btbb_packet_get_lt_addr(const btbb_packet * pkt)524 uint8_t btbb_packet_get_lt_addr(const btbb_packet* pkt)
525 {
526 return pkt->packet_lt_addr;
527 }
528
btbb_packet_get_header_flags(const btbb_packet * pkt)529 uint8_t btbb_packet_get_header_flags(const btbb_packet* pkt)
530 {
531 return pkt->packet_flags;
532 }
533
btbb_packet_get_hec(const btbb_packet * pkt)534 uint8_t btbb_packet_get_hec(const btbb_packet* pkt)
535 {
536 return pkt->packet_hec;
537 }
538
btbb_packet_get_header_packed(const btbb_packet * pkt)539 uint32_t btbb_packet_get_header_packed(const btbb_packet* pkt)
540 {
541 return air_to_host32(&pkt->packet_header[0], 18);
542 }
543
544 /* Reverse the bits in a byte */
reverse(char byte)545 static uint8_t reverse(char byte)
546 {
547 return (byte & 0x80) >> 7 | (byte & 0x40) >> 5 | (byte & 0x20) >> 3 | (byte & 0x10) >> 1 | (byte & 0x08) << 1 | (byte & 0x04) << 3 | (byte & 0x02) << 5 | (byte & 0x01) << 7;
548 }
549
550
551 /* Decode 1/3 rate FEC, three like symbols in a row */
unfec13(char * input,char * output,int length)552 static int unfec13(char *input, char *output, int length)
553 {
554 int a, b, c, i;
555 int be = 0; /* bit errors */
556
557 for (i = 0; i < length; i++) {
558 a = 3 * i;
559 b = a + 1;
560 c = a + 2;
561 output[i] = ((input[a] & input[b]) | (input[b] & input[c]) |
562 (input[c] & input[a]));
563 be += ((input[a] ^ input[b]) | (input[b] ^ input[c]) |
564 (input[c] ^ input[a]));
565 }
566
567 return (be < (length / 4));
568 }
569
570 /* encode 10 bits with 2/3 rate FEC code, a (15,10) shortened Hamming code */
fec23(uint16_t data)571 static uint16_t fec23(uint16_t data)
572 {
573 int i;
574 uint16_t codeword = 0;
575
576 /* host order, not air order */
577 for (i = 0; i < 10; i++)
578 if (data & (1 << i))
579 codeword ^= fec23_gen_matrix[i];
580
581 return codeword;
582 }
583
584 /* Decode 2/3 rate FEC, a (15,10) shortened Hamming code */
unfec23(char * input,int length)585 static char *unfec23(char *input, int length)
586 {
587 /* input points to the input data
588 * length is length in bits of the data
589 * before it was encoded with fec2/3 */
590 int iptr, optr, count;
591 char* output;
592 uint8_t diff, check;
593 uint16_t data, codeword;
594
595 diff = length % 10;
596 // padding at end of data
597 if(0!=diff)
598 length += (10 - diff);
599
600 output = (char *) malloc(length);
601
602 for (iptr = 0, optr = 0; optr<length; iptr += 15, optr += 10) {
603 // copy data to output
604 for(count=0;count<10;count++)
605 output[optr+count] = input[iptr+count];
606
607 // grab data and error check in host format
608 data = air_to_host16(input+iptr, 10);
609 check = air_to_host8(input+iptr+10, 5);
610
611 // call fec23 on data to generate the codeword
612 codeword = fec23(data);
613 diff = check ^ (codeword >> 10);
614
615 /* no errors or single bit errors (errors in the parity bit):
616 * (a strong hint it's a real packet)
617 * Otherwise we need to corret the output*/
618 if (diff & (diff - 1)) {
619 switch (diff) {
620 /* comments are the bit that's wrong and the value
621 * of diff in air order, from the BT spec */
622 // 1000000000 11010
623 case 0x0b: output[optr] ^= 1; break;
624 // 0100000000 01101
625 case 0x16: output[optr+1] ^= 1; break;
626 // 0010000000 11100
627 case 0x07: output[optr+2] ^= 1; break;
628 // 0001000000 01110
629 case 0x0e: output[optr+3] ^= 1; break;
630 // 0000100000 00111
631 case 0x1c: output[optr+4] ^= 1; break;
632 // 0000010000 11001
633 case 0x13: output[optr+5] ^= 1; break;
634 // 0000001000 10110
635 case 0x0d: output[optr+6] ^= 1; break;
636 // 0000000100 01011
637 case 0x1a: output[optr+7] ^= 1; break;
638 // 0000000010 11111
639 case 0x1f: output[optr+8] ^= 1; break;
640 // 0000000001 10101
641 case 0x15: output[optr+9] ^= 1; break;
642 /* not one of these errors, probably multiple bit errors
643 * or maybe not a real packet, safe to drop it? */
644 default: free(output); return 0;
645 }
646 }
647 }
648 return output;
649 }
650
651
652 /* Remove the whitening from an air order array */
unwhiten(char * input,char * output,int clock,int length,int skip,btbb_packet * pkt)653 static void unwhiten(char* input, char* output, int clock, int length, int skip, btbb_packet* pkt)
654 {
655 int count, index;
656 index = INDICES[clock & 0x3f];
657 index += skip;
658 index %= 127;
659
660 for(count = 0; count < length; count++)
661 {
662 /* unwhiten if whitened, otherwise just copy input to output */
663 output[count] = btbb_packet_get_flag(pkt, BTBB_WHITENED) ?
664 input[count] ^ WHITENING_DATA[index] : input[count];
665 index += 1;
666 index %= 127;
667 }
668 }
669
670 /* Pointer to start of packet, length of packet in bits, UAP */
crcgen(char * payload,int length,int UAP)671 static uint16_t crcgen(char *payload, int length, int UAP)
672 {
673 char bit;
674 uint16_t reg, count;
675
676 reg = (reverse(UAP) << 8) & 0xff00;
677 for(count = 0; count < length; count++)
678 {
679 bit = payload[count];
680
681 reg = (reg >> 1) | (((reg & 0x0001) ^ (bit & 0x01))<<15);
682
683 /*Bit 5*/
684 reg ^= ((reg & 0x8000)>>5);
685
686 /*Bit 12*/
687 reg ^= ((reg & 0x8000)>>12);
688 }
689 return reg;
690 }
691
692 /* extract UAP by reversing the HEC computation */
uap_from_hec(uint16_t data,uint8_t hec)693 static uint8_t uap_from_hec(uint16_t data, uint8_t hec)
694 {
695 int i;
696
697 for (i = 9; i >= 0; i--) {
698 /* 0x65 is xor'd if MSB is 1, else 0x00 (which does nothing) */
699 if (hec & 0x80)
700 hec ^= 0x65;
701
702 hec = (hec << 1) | (((hec >> 7) ^ (data >> i)) & 0x01);
703 }
704 return reverse(hec);
705 }
706
707 /* check if the packet's CRC is correct for a given clock (CLK1-6) */
crc_check(int clock,btbb_packet * pkt)708 int crc_check(int clock, btbb_packet* pkt)
709 {
710 /*
711 * return value of 1 represents inconclusive result (default)
712 * return value > 1 represents positive result (e.g. CRC match)
713 * return value of 0 represents negative result (e.g. CRC failure without
714 * the possibility that we have assumed the wrong logical transport)
715 */
716 int retval = 1;
717
718 switch(pkt->packet_type)
719 {
720 case PACKET_TYPE_FHS:
721 retval = fhs(clock, pkt);
722 break;
723
724 case PACKET_TYPE_DV:
725 case PACKET_TYPE_DM1:
726 case PACKET_TYPE_DM3:
727 case PACKET_TYPE_DM5:
728 retval = DM(clock, pkt);
729 break;
730
731 case PACKET_TYPE_DH1:
732 case PACKET_TYPE_DH3:
733 case PACKET_TYPE_DH5:
734 retval = DH(clock, pkt);
735 break;
736
737 case PACKET_TYPE_HV3: /* EV3 */
738 retval = EV3(clock, pkt);
739 break;
740 case PACKET_TYPE_EV4:
741 retval = EV4(clock, pkt);
742 break;
743 case PACKET_TYPE_EV5:
744 retval = EV5(clock, pkt);
745 break;
746
747 case PACKET_TYPE_HV1:
748 retval = HV(clock, pkt);
749 break;
750
751 /* some types can't help us */
752 default:
753 break;
754 }
755 /*
756 * never return a zero result unless this is a FHS, DM1, or HV1. any
757 * other type could have actually been something else (another logical
758 * transport)
759 */
760 if (retval == 0 && (pkt->packet_type != 2 && pkt->packet_type != 3 &&
761 pkt->packet_type != 5))
762 return 1;
763
764 /* EV3 and EV5 have a relatively high false positive rate */
765 if (retval > 1 && (pkt->packet_type == 7 || pkt->packet_type == 13))
766 return 1;
767
768 return retval;
769 }
770
771 /* verify the payload CRC */
payload_crc(btbb_packet * pkt)772 static int payload_crc(btbb_packet* pkt)
773 {
774 uint16_t crc; /* CRC calculated from payload data */
775 uint16_t check; /* CRC supplied by packet */
776
777 crc = crcgen(pkt->payload, (pkt->payload_length - 2) * 8, pkt->UAP);
778 check = air_to_host16(&pkt->payload[(pkt->payload_length - 2) * 8], 16);
779
780 return (crc == check);
781 }
782
fhs(int clock,btbb_packet * pkt)783 int fhs(int clock, btbb_packet* pkt)
784 {
785 /* skip the access code and packet header */
786 char *stream = pkt->symbols + 122;
787 /* number of symbols remaining after access code and packet header */
788 int size = pkt->length - 122;
789
790 pkt->payload_length = 20;
791
792 if (size < pkt->payload_length * 12)
793 return 1; //FIXME should throw exception
794
795 char *corrected = unfec23(stream, pkt->payload_length * 8);
796 if (!corrected)
797 return 0;
798
799 /* try to unwhiten with known clock bits */
800 unwhiten(corrected, pkt->payload, clock, pkt->payload_length * 8, 18, pkt);
801 if (payload_crc(pkt)) {
802 free(corrected);
803 return 1000;
804 }
805
806 /* try all 32 possible X-input values instead */
807 for (clock = 32; clock < 64; clock++) {
808 unwhiten(corrected, pkt->payload, clock, pkt->payload_length * 8, 18, pkt);
809 if (payload_crc(pkt)) {
810 free(corrected);
811 return 1000;
812 }
813 }
814
815 /* failed to unwhiten */
816 free(corrected);
817 return 0;
818 }
819
820 /* decode payload header, return value indicates success */
decode_payload_header(char * stream,int clock,int header_bytes,int size,int fec,btbb_packet * pkt)821 static int decode_payload_header(char *stream, int clock, int header_bytes, int size, int fec, btbb_packet* pkt)
822 {
823 if(header_bytes == 2)
824 {
825 if(size < 16)
826 return 0; //FIXME should throw exception
827 if(fec) {
828 if(size < 30)
829 return 0; //FIXME should throw exception
830 char *corrected = unfec23(stream, 16);
831 if (!corrected)
832 return 0;
833 unwhiten(corrected, pkt->payload_header, clock, 16, 18, pkt);
834 free(corrected);
835 } else {
836 unwhiten(stream, pkt->payload_header, clock, 16, 18, pkt);
837 }
838 /* payload length is payload body length + 2 bytes payload header + 2 bytes CRC */
839 pkt->payload_length = air_to_host16(&pkt->payload_header[3], 10) + 4;
840 } else {
841 if(size < 8)
842 return 0; //FIXME should throw exception
843 if(fec) {
844 if(size < 15)
845 return 0; //FIXME should throw exception
846 char *corrected = unfec23(stream, 8);
847 if (!corrected)
848 return 0;
849 unwhiten(corrected, pkt->payload_header, clock, 8, 18, pkt);
850 free(corrected);
851 } else {
852 unwhiten(stream, pkt->payload_header, clock, 8, 18, pkt);
853 }
854 /* payload length is payload body length + 1 byte payload header + 2 bytes CRC */
855 pkt->payload_length = air_to_host8(&pkt->payload_header[3], 5) + 3;
856 }
857 /* Try to set the max payload length to a sensible value,
858 * especially when using strange data
859 */
860 int max_length = 0;
861 switch(pkt->packet_type) {
862 case PACKET_TYPE_DM1:
863 max_length = 20;
864 break;
865 case PACKET_TYPE_DH1:
866 /* assuming DH1 but could be 2-DH1 (58) */
867 max_length = 30;
868 break;
869 case PACKET_TYPE_DV:
870 /* assuming DV but could be 3-DH1 (87) */
871 max_length = 12; /* + 10bytes of voice data */
872 break;
873 case PACKET_TYPE_DM3:
874 /* assuming DM3 but could be 2-DH3 (371) */
875 max_length = 125;
876 break;
877 case PACKET_TYPE_DH3:
878 /* assuming DH3 but could be 3-DH3 (556) */
879 max_length = 187;
880 break;
881 case PACKET_TYPE_DM5:
882 /* assuming DM5 but could be 2-DH5 (683) */
883 max_length = 228;
884 break;
885 case PACKET_TYPE_DH5:
886 /* assuming DH5 but could be 3-DH5 (1025) */
887 max_length = 343;
888 break;
889 }
890 pkt->payload_length = MIN(pkt->payload_length, max_length);
891 pkt->payload_llid = air_to_host8(&pkt->payload_header[0], 2);
892 pkt->payload_flow = air_to_host8(&pkt->payload_header[2], 1);
893 pkt->payload_header_length = header_bytes;
894 return 1;
895 }
896
897 /* DM 1/3/5 packet (and DV)*/
DM(int clock,btbb_packet * pkt)898 int DM(int clock, btbb_packet* pkt)
899 {
900 int bitlength;
901 /* number of bytes in the payload header */
902 int header_bytes = 2;
903 /* maximum payload length */
904 int max_length;
905 /* skip the access code and packet header */
906 char *stream = pkt->symbols + 122;
907 /* number of symbols remaining after access code and packet header */
908 int size = pkt->length - 122;
909
910 switch(pkt->packet_type)
911 {
912 case PACKET_TYPE_DV:
913 /* skip 80 voice bits, then treat the rest like a DM1 */
914 stream += 80;
915 size -= 80;
916 header_bytes = 1;
917 /* I don't think the length of the voice field ("synchronous data
918 * field") is included in the length indicated by the payload
919 * header in the data field ("asynchronous data field"), but I
920 * could be wrong.
921 */
922 max_length = 12;
923 break;
924 case PACKET_TYPE_DM1:
925 header_bytes = 1;
926 max_length = 20;
927 break;
928 case PACKET_TYPE_DM3:
929 max_length = 125;
930 break;
931 case PACKET_TYPE_DM5:
932 max_length = 228;
933 break;
934 default: /* not a DM1/3/5 or DV */
935 return 0;
936 }
937 if(!decode_payload_header(stream, clock, header_bytes, size, 1, pkt))
938 return 0;
939 /* check that the length indicated in the payload header is within spec */
940 if(pkt->payload_length > max_length)
941 /* could be encrypted */
942 return 1;
943 bitlength = pkt->payload_length*8;
944 if(bitlength > size)
945 return 1; //FIXME should throw exception
946
947 char *corrected = unfec23(stream, bitlength);
948 if (!corrected)
949 return 0;
950 unwhiten(corrected, pkt->payload, clock, bitlength, 18, pkt);
951 free(corrected);
952
953 if (payload_crc(pkt))
954 return 10;
955
956 /* could be encrypted */
957 return 2;
958 }
959
960 /* DH 1/3/5 packet (and AUX1) */
961 /* similar to DM 1/3/5 but without FEC */
DH(int clock,btbb_packet * pkt)962 int DH(int clock, btbb_packet* pkt)
963 {
964 int bitlength;
965 /* number of bytes in the payload header */
966 int header_bytes = 2;
967 /* maximum payload length */
968 int max_length;
969 /* skip the access code and packet header */
970 char *stream = pkt->symbols + 122;
971 /* number of symbols remaining after access code and packet header */
972 int size = pkt->length - 122;
973
974 switch(pkt->packet_type)
975 {
976 case PACKET_TYPE_AUX1:
977 case PACKET_TYPE_DH1:
978 header_bytes = 1;
979 max_length = 30;
980 break;
981 case PACKET_TYPE_DH3:
982 max_length = 187;
983 break;
984 case PACKET_TYPE_DH5:
985 max_length = 343;
986 break;
987 default: /* not a DH1/3/5 */
988 return 0;
989 }
990 if(!decode_payload_header(stream, clock, header_bytes, size, 0, pkt))
991 return 0;
992 /* check that the length indicated in the payload header is within spec */
993 if(pkt->payload_length > max_length)
994 /* could be encrypted */
995 return 1;
996 bitlength = pkt->payload_length*8;
997 if(bitlength > size)
998 return 1; //FIXME should throw exception
999
1000 unwhiten(stream, pkt->payload, clock, bitlength, 18, pkt);
1001
1002 /* AUX1 has no CRC */
1003 if (pkt->packet_type == 9)
1004 return 2;
1005
1006 if (payload_crc(pkt))
1007 return 10;
1008
1009 /* could be encrypted */
1010 return 2;
1011 }
1012
EV3(int clock,btbb_packet * pkt)1013 int EV3(int clock, btbb_packet* pkt)
1014 {
1015 /* skip the access code and packet header */
1016 char *stream = pkt->symbols + 122;
1017
1018 /* number of symbols remaining after access code and packet header */
1019 int size = pkt->length - 122;
1020
1021 /* maximum payload length is 30 bytes + 2 bytes CRC */
1022 int maxlength = 32;
1023
1024 /* number of bits we have decoded */
1025 int bits;
1026
1027 /* check CRC for any integer byte length up to maxlength */
1028 for (pkt->payload_length = 0;
1029 pkt->payload_length < maxlength; pkt->payload_length++) {
1030
1031 bits = pkt->payload_length * 8;
1032
1033 /* unwhiten next byte */
1034 if ((bits + 8) > size)
1035 return 1; //FIXME should throw exception
1036 unwhiten(stream, pkt->payload + bits, clock, 8, 18 + bits, pkt);
1037
1038 if ((pkt->payload_length > 2) && (payload_crc(pkt)))
1039 return 10;
1040 }
1041 return 2;
1042 }
1043
EV4(int clock,btbb_packet * pkt)1044 int EV4(int clock, btbb_packet* pkt)
1045 {
1046 char *corrected;
1047
1048 /* skip the access code and packet header */
1049 char *stream = pkt->symbols + 122;
1050
1051 /* number of symbols remaining after access code and packet header */
1052 int size = pkt->length - 122;
1053
1054 /*
1055 * maximum payload length is 120 bytes + 2 bytes CRC
1056 * after FEC2/3, this results in a maximum of 1470 symbols
1057 */
1058 int maxlength = 1470;
1059
1060 /*
1061 * minumum payload length is 1 bytes + 2 bytes CRC
1062 * after FEC2/3, this results in a minimum of 45 symbols
1063 */
1064 int minlength = 45;
1065
1066 int syms = 0; /* number of symbols we have decoded */
1067 int bits = 0; /* number of payload bits we have decoded */
1068
1069 pkt->payload_length = 1;
1070
1071 while (syms < maxlength) {
1072
1073 /* unfec/unwhiten next block (15 symbols -> 10 bits) */
1074 if (syms + 15 > size)
1075 return 1; //FIXME should throw exception
1076 corrected = unfec23(stream + syms, 10);
1077 if (!corrected) {
1078 free(corrected);
1079 if (syms < minlength)
1080 return 0;
1081 else
1082 return 1;
1083 }
1084 unwhiten(corrected, pkt->payload + bits, clock, 10, 18 + bits, pkt);
1085 free(corrected);
1086
1087 /* check CRC one byte at a time */
1088 while (pkt->payload_length * 8 <= bits) {
1089 if (payload_crc(pkt))
1090 return 10;
1091 pkt->payload_length++;
1092 }
1093 syms += 15;
1094 bits += 10;
1095 }
1096 return 2;
1097 }
1098
EV5(int clock,btbb_packet * pkt)1099 int EV5(int clock, btbb_packet* pkt)
1100 {
1101 /* skip the access code and packet header */
1102 char *stream = pkt->symbols + 122;
1103
1104 /* number of symbols remaining after access code and packet header */
1105 int size = pkt->length - 122;
1106
1107 /* maximum payload length is 180 bytes + 2 bytes CRC */
1108 int maxlength = 182;
1109
1110 /* number of bits we have decoded */
1111 int bits;
1112
1113 /* check CRC for any integer byte length up to maxlength */
1114 for (pkt->payload_length = 0;
1115 pkt->payload_length < maxlength; pkt->payload_length++) {
1116
1117 bits = pkt->payload_length * 8;
1118
1119 /* unwhiten next byte */
1120 if ((bits + 8) > size)
1121 return 1; //FIXME should throw exception
1122 unwhiten(stream, pkt->payload + bits, clock, 8, 18 + bits, pkt);
1123
1124 if ((pkt->payload_length > 2) && (payload_crc(pkt)))
1125 return 10;
1126 }
1127 return 2;
1128 }
1129
1130 /* HV packet type payload parser */
HV(int clock,btbb_packet * pkt)1131 int HV(int clock, btbb_packet* pkt)
1132 {
1133 /* skip the access code and packet header */
1134 char *stream = pkt->symbols + 122;
1135 /* number of symbols remaining after access code and packet header */
1136 int size = pkt->length - 122;
1137
1138 pkt->payload_header_length = 0;
1139 if(size < 240) {
1140 pkt->payload_length = 0;
1141 return 1; //FIXME should throw exception
1142 }
1143
1144 switch (pkt->packet_type) {
1145 case PACKET_TYPE_HV1:
1146 {
1147 char corrected[80];
1148 if (!unfec13(stream, corrected, 80))
1149 return 0;
1150 pkt->payload_length = 10;
1151 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 1);
1152 unwhiten(corrected, pkt->payload, clock, pkt->payload_length*8, 18, pkt);
1153 }
1154 break;
1155 case PACKET_TYPE_HV2:
1156 {
1157 char *corrected = unfec23(stream, 160);
1158 if (!corrected)
1159 return 0;
1160 pkt->payload_length = 20;
1161 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 1);
1162 unwhiten(corrected, pkt->payload, clock, pkt->payload_length*8, 18, pkt);
1163 free(corrected);
1164 }
1165 break;
1166 case PACKET_TYPE_HV3:
1167 pkt->payload_length = 30;
1168 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 1);
1169 unwhiten(stream, pkt->payload, clock, pkt->payload_length*8, 18, pkt);
1170 break;
1171 }
1172
1173 return 2;
1174 }
1175 /* try a clock value (CLK1-6) to unwhiten packet header,
1176 * sets resultant p->packet_type and p->UAP, returns UAP.
1177 */
try_clock(int clock,btbb_packet * pkt)1178 uint8_t try_clock(int clock, btbb_packet* pkt)
1179 {
1180 /* skip 72 bit access code */
1181 char *stream = pkt->symbols + 68;
1182 /* 18 bit packet header */
1183 char header[18];
1184 char unwhitened[18];
1185
1186 if (!unfec13(stream, header, 18))
1187 return 0;
1188 unwhiten(header, unwhitened, clock, 18, 0, pkt);
1189 uint16_t hdr_data = air_to_host16(unwhitened, 10);
1190 uint8_t hec = air_to_host8(&unwhitened[10], 8);
1191 pkt->UAP = uap_from_hec(hdr_data, hec);
1192 pkt->packet_type = air_to_host8(&unwhitened[3], 4);
1193
1194 return pkt->UAP;
1195 }
1196
1197 /* decode the packet header */
btbb_decode_header(btbb_packet * pkt)1198 int btbb_decode_header(btbb_packet* pkt)
1199 {
1200 /* skip 72 bit access code */
1201 char *stream = pkt->symbols + 68;
1202 /* 18 bit packet header */
1203 char header[18];
1204 uint8_t UAP;
1205
1206 if (btbb_packet_get_flag(pkt, BTBB_CLK6_VALID) && unfec13(stream, header, 18)) {
1207 unwhiten(header, pkt->packet_header, pkt->clkn, 18, 0, pkt);
1208 uint16_t hdr_data = air_to_host16(pkt->packet_header, 10);
1209 uint8_t hec = air_to_host8(&pkt->packet_header[10], 8);
1210 UAP = uap_from_hec(hdr_data, hec);
1211 if (UAP == pkt->UAP) {
1212 pkt->packet_lt_addr = air_to_host8(&pkt->packet_header[0], 3);
1213 pkt->packet_type = air_to_host8(&pkt->packet_header[3], 4);
1214 pkt->packet_flags = air_to_host8(&pkt->packet_header[7], 3);
1215 pkt->packet_hec = hec;
1216 return 1;
1217 }
1218 }
1219
1220 return 0;
1221 }
1222
btbb_decode_payload(btbb_packet * pkt)1223 int btbb_decode_payload(btbb_packet* pkt)
1224 {
1225 int rv = 0;
1226 pkt->payload_header_length = 0;
1227
1228 switch(pkt->packet_type)
1229 {
1230 case PACKET_TYPE_NULL:
1231 /* no payload to decode */
1232 pkt->payload_length = 0;
1233 rv = 1;
1234 break;
1235 case PACKET_TYPE_POLL:
1236 /* no payload to decode */
1237 pkt->payload_length = 0;
1238 rv = 1;
1239 break;
1240 case PACKET_TYPE_FHS:
1241 rv = fhs(pkt->clkn, pkt);
1242 break;
1243 case PACKET_TYPE_DM1:
1244 rv = DM(pkt->clkn, pkt);
1245 break;
1246 case PACKET_TYPE_DH1:
1247 /* assuming DH1 but could be 2-DH1 */
1248 rv = DH(pkt->clkn, pkt);
1249 break;
1250 case PACKET_TYPE_HV1:
1251 rv = HV(pkt->clkn, pkt);
1252 break;
1253 case PACKET_TYPE_HV2:
1254 rv = HV(pkt->clkn, pkt);
1255 break;
1256 case PACKET_TYPE_HV3: /* HV3/EV3/3-EV3 */
1257 /* decode as EV3 if CRC checks out */
1258 if ((rv = EV3(pkt->clkn, pkt)) <= 1)
1259 /* otherwise assume HV3 */
1260 rv = HV(pkt->clkn, pkt);
1261 /* don't know how to decode 3-EV3 */
1262 break;
1263 case PACKET_TYPE_DV:
1264 /* assuming DV but could be 3-DH1 */
1265 rv = DM(pkt->clkn, pkt);
1266 break;
1267 case PACKET_TYPE_AUX1:
1268 rv = DH(pkt->clkn, pkt);
1269 break;
1270 case PACKET_TYPE_DM3:
1271 /* assuming DM3 but could be 2-DH3 */
1272 rv = DM(pkt->clkn, pkt);
1273 break;
1274 case PACKET_TYPE_DH3:
1275 /* assuming DH3 but could be 3-DH3 */
1276 rv = DH(pkt->clkn, pkt);
1277 break;
1278 case PACKET_TYPE_EV4:
1279 /* assuming EV4 but could be 2-EV5 */
1280 rv = EV4(pkt->clkn, pkt);
1281 break;
1282 case PACKET_TYPE_EV5:
1283 /* assuming EV5 but could be 3-EV5 */
1284 rv = EV5(pkt->clkn, pkt);
1285 break;
1286 case PACKET_TYPE_DM5:
1287 /* assuming DM5 but could be 2-DH5 */
1288 rv = DM(pkt->clkn, pkt);
1289 break;
1290 case PACKET_TYPE_DH5:
1291 /* assuming DH5 but could be 3-DH5 */
1292 rv = DH(pkt->clkn, pkt);
1293 break;
1294 }
1295 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 1);
1296 return rv;
1297 }
1298
1299 /* decode the whole packet */
btbb_decode(btbb_packet * pkt)1300 int btbb_decode(btbb_packet* pkt)
1301 {
1302 int rv = 0;
1303
1304 btbb_packet_set_flag(pkt, BTBB_HAS_PAYLOAD, 0);
1305
1306 if (btbb_decode_header(pkt)) {
1307 rv = btbb_decode_payload(pkt);
1308 }
1309
1310 /* If we were successful, print the packet */
1311 if(rv > 0) {
1312 printf("Packet decoded with clock 0x%02x (rv=%d)\n", pkt->clkn & 0x3f, rv);
1313 btbb_print_packet(pkt);
1314 }
1315
1316 return rv;
1317 }
1318
1319 /* print packet information */
btbb_print_packet(const btbb_packet * pkt)1320 void btbb_print_packet(const btbb_packet* pkt)
1321 {
1322 if (btbb_packet_get_flag(pkt, BTBB_HAS_PAYLOAD)) {
1323 printf(" Type: %s\n", TYPE_NAMES[pkt->packet_type]);
1324 if (pkt->payload_header_length > 0) {
1325 printf(" LT_ADDR: %d\n", pkt->packet_lt_addr);
1326 printf(" LLID: %d\n", pkt->payload_llid);
1327 printf(" flow: %d\n", pkt->payload_flow);
1328 printf(" payload length: %d\n", pkt->payload_length);
1329 }
1330 if (pkt->payload_length) {
1331 printf(" Data: ");
1332 int i;
1333 for(i=0; i<pkt->payload_length; i++)
1334 printf(" %02x", air_to_host8(pkt->payload + 8*i, 8));
1335 printf("\n");
1336 }
1337 }
1338 }
1339
tun_format(btbb_packet * pkt)1340 char *tun_format(btbb_packet* pkt)
1341 {
1342 /* include 6 bytes for meta data, 3 bytes for packet header */
1343 int length = 9 + pkt->payload_length;
1344 char *tun_format = (char *) malloc(length);
1345 int i;
1346
1347 /* meta data */
1348 tun_format[0] = pkt->clkn & 0xff;
1349 tun_format[1] = (pkt->clkn >> 8) & 0xff;
1350 tun_format[2] = (pkt->clkn >> 16) & 0xff;
1351 tun_format[3] = (pkt->clkn >> 24) & 0xff;
1352 tun_format[4] = pkt->channel;
1353 tun_format[5] = btbb_packet_get_flag(pkt, BTBB_CLK27_VALID) |
1354 (btbb_packet_get_flag(pkt, BTBB_NAP_VALID) << 1);
1355
1356 /* packet header modified to fit byte boundaries */
1357 /* lt_addr and type */
1358 tun_format[6] = (char) air_to_host8(&pkt->packet_header[0], 7);
1359 /* flags */
1360 tun_format[7] = (char) air_to_host8(&pkt->packet_header[7], 3);
1361 /* HEC */
1362 tun_format[8] = (char) air_to_host8(&pkt->packet_header[10], 8);
1363
1364 for(i=0;i<pkt->payload_length;i++)
1365 tun_format[i+9] = (char) air_to_host8(&pkt->payload[i*8], 8);
1366
1367 return tun_format;
1368 }
1369
1370 /* check to see if the packet has a header */
btbb_header_present(const btbb_packet * pkt)1371 int btbb_header_present(const btbb_packet* pkt)
1372 {
1373 /* skip to last bit of sync word */
1374 const char *stream = pkt->symbols + 63;
1375 int be = 0; /* bit errors */
1376 char msb; /* most significant (last) bit of sync word */
1377 int a, b, c;
1378
1379 /* check that we have enough symbols */
1380 if (pkt->length < 122)
1381 return 0;
1382
1383 /* check that the AC trailer is correct */
1384 msb = stream[0];
1385 be += stream[1] ^ !msb;
1386 be += stream[2] ^ msb;
1387 be += stream[3] ^ !msb;
1388 be += stream[4] ^ msb;
1389
1390 /*
1391 * Each bit of the 18 bit header is repeated three times. Without
1392 * checking the correctness of any particular bit, just count the
1393 * number of times three symbols in a row don't all agree.
1394 */
1395 stream += 5;
1396 for (a = 0; a < 54; a += 3) {
1397 b = a + 1;
1398 c = a + 2;
1399 be += ((stream[a] ^ stream[b]) |
1400 (stream[b] ^ stream[c]) | (stream[c] ^ stream[a]));
1401 }
1402
1403 /*
1404 * Few bit errors indicates presence of a header. Many bit errors
1405 * indicates no header is present (i.e. it is an ID packet).
1406 */
1407 return (be < ID_THRESHOLD);
1408 }
1409
1410 /* extract LAP from FHS payload */
lap_from_fhs(btbb_packet * pkt)1411 uint32_t lap_from_fhs(btbb_packet* pkt)
1412 {
1413 /* caller should check got_payload() and get_type() */
1414 return air_to_host32(&pkt->payload[34], 24);
1415 }
1416
1417 /* extract UAP from FHS payload */
uap_from_fhs(btbb_packet * pkt)1418 uint8_t uap_from_fhs(btbb_packet* pkt)
1419 {
1420 /* caller should check got_payload() and get_type() */
1421 return air_to_host8(&pkt->payload[64], 8);
1422 }
1423
1424 /* extract NAP from FHS payload */
nap_from_fhs(btbb_packet * pkt)1425 uint16_t nap_from_fhs(btbb_packet* pkt)
1426 {
1427 /* caller should check got_payload() and get_type() */
1428 return air_to_host16(&pkt->payload[72], 16);
1429 }
1430
1431 /* extract clock from FHS payload */
clock_from_fhs(btbb_packet * pkt)1432 uint32_t clock_from_fhs(btbb_packet* pkt)
1433 {
1434 /*
1435 * caller should check got_payload() and get_type()
1436 *
1437 * This is CLK2-27 (units of 1.25 ms).
1438 * CLK0 and CLK1 are implicitly zero.
1439 */
1440 return air_to_host32(&pkt->payload[115], 26);
1441 }
1442