1 /*
2 *
3 * BlueZ - Bluetooth protocol stack for Linux
4 *
5 * Copyright (C) 2004-2011 Marcel Holtmann <marcel@holtmann.org>
6 *
7 *
8 * This program is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation; either version 2 of the License, or
11 * (at your option) any later version.
12 *
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU General Public License for more details.
17 *
18 * You should have received a copy of the GNU General Public License
19 * along with this program; if not, write to the Free Software
20 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 *
22 */
23
24 #ifdef HAVE_CONFIG_H
25 #include <config.h>
26 #endif
27
28 #include <stdio.h>
29 #include <errno.h>
30 #include <ctype.h>
31 #include <sys/socket.h>
32
33 #include <bluetooth/bluetooth.h>
34 #include <bluetooth/hci.h>
35 #include <bluetooth/hci_lib.h>
36
37 #include "parser.h"
38
39 #define LMP_U8(frm) (get_u8(frm))
40 #define LMP_U16(frm) (btohs(htons(get_u16(frm))))
41 #define LMP_U32(frm) (btohl(htonl(get_u32(frm))))
42
43 static enum {
44 IN_RAND,
45 COMB_KEY_M,
46 COMB_KEY_S,
47 AU_RAND_M,
48 AU_RAND_S,
49 SRES_M,
50 SRES_S,
51 } pairing_state = IN_RAND;
52
53 static struct {
54 uint8_t in_rand[16];
55 uint8_t comb_key_m[16];
56 uint8_t comb_key_s[16];
57 uint8_t au_rand_m[16];
58 uint8_t au_rand_s[16];
59 uint8_t sres_m[4];
60 uint8_t sres_s[4];
61 } pairing_data;
62
pairing_data_dump(void)63 static inline void pairing_data_dump(void)
64 {
65 int i;
66
67 p_indent(6, NULL);
68 printf("IN_RAND ");
69 for (i = 0; i < 16; i++)
70 printf("%2.2x", pairing_data.in_rand[i]);
71 printf("\n");
72
73 p_indent(6, NULL);
74 printf("COMB_KEY ");
75 for (i = 0; i < 16; i++)
76 printf("%2.2x", pairing_data.comb_key_m[i]);
77 printf(" (M)\n");
78
79 p_indent(6, NULL);
80 printf("COMB_KEY ");
81 for (i = 0; i < 16; i++)
82 printf("%2.2x", pairing_data.comb_key_s[i]);
83 printf(" (S)\n");
84
85 p_indent(6, NULL);
86 printf("AU_RAND ");
87 for (i = 0; i < 16; i++)
88 printf("%2.2x", pairing_data.au_rand_m[i]);
89 printf(" SRES ");
90 for (i = 0; i < 4; i++)
91 printf("%2.2x", pairing_data.sres_m[i]);
92 printf(" (M)\n");
93
94 p_indent(6, NULL);
95 printf("AU_RAND ");
96 for (i = 0; i < 16; i++)
97 printf("%2.2x", pairing_data.au_rand_s[i]);
98 printf(" SRES ");
99 for (i = 0; i < 4; i++)
100 printf("%2.2x", pairing_data.sres_s[i]);
101 printf(" (S)\n");
102 }
103
in_rand(struct frame * frm)104 static inline void in_rand(struct frame *frm)
105 {
106 uint8_t *val = frm->ptr;
107
108 memcpy(pairing_data.in_rand, val, 16);
109 pairing_state = COMB_KEY_M;
110 }
111
comb_key(struct frame * frm)112 static inline void comb_key(struct frame *frm)
113 {
114 uint8_t *val = frm->ptr;
115
116 switch (pairing_state) {
117 case COMB_KEY_M:
118 memcpy(pairing_data.comb_key_m, val, 16);
119 pairing_state = COMB_KEY_S;
120 break;
121 case COMB_KEY_S:
122 memcpy(pairing_data.comb_key_s, val, 16);
123 pairing_state = AU_RAND_M;
124 break;
125 default:
126 pairing_state = IN_RAND;
127 break;
128 }
129 }
130
au_rand(struct frame * frm)131 static inline void au_rand(struct frame *frm)
132 {
133 uint8_t *val = frm->ptr;
134
135 switch (pairing_state) {
136 case AU_RAND_M:
137 memcpy(pairing_data.au_rand_m, val, 16);
138 pairing_state = SRES_M;
139 break;
140 case AU_RAND_S:
141 memcpy(pairing_data.au_rand_s, val, 16);
142 pairing_state = SRES_S;
143 break;
144 default:
145 pairing_state = IN_RAND;
146 break;
147 }
148 }
149
sres(struct frame * frm)150 static inline void sres(struct frame *frm)
151 {
152 uint8_t *val = frm->ptr;
153
154 switch (pairing_state) {
155 case SRES_M:
156 memcpy(pairing_data.sres_m, val, 4);
157 pairing_state = AU_RAND_S;
158 break;
159 case SRES_S:
160 memcpy(pairing_data.sres_s, val, 4);
161 pairing_state = IN_RAND;
162 pairing_data_dump();
163 break;
164 default:
165 pairing_state = IN_RAND;
166 break;
167 }
168 }
169
opcode2str(uint16_t opcode)170 static char *opcode2str(uint16_t opcode)
171 {
172 switch (opcode) {
173 case 1:
174 return "name_req";
175 case 2:
176 return "name_res";
177 case 3:
178 return "accepted";
179 case 4:
180 return "not_accepted";
181 case 5:
182 return "clkoffset_req";
183 case 6:
184 return "clkoffset_res";
185 case 7:
186 return "detach";
187 case 8:
188 return "in_rand";
189 case 9:
190 return "comb_key";
191 case 10:
192 return "unit_key";
193 case 11:
194 return "au_rand";
195 case 12:
196 return "sres";
197 case 13:
198 return "temp_rand";
199 case 14:
200 return "temp_key";
201 case 15:
202 return "encryption_mode_req";
203 case 16:
204 return "encryption_key_size_req";
205 case 17:
206 return "start_encryption_req";
207 case 18:
208 return "stop_encryption_req";
209 case 19:
210 return "switch_req";
211 case 20:
212 return "hold";
213 case 21:
214 return "hold_req";
215 case 22:
216 return "sniff";
217 case 23:
218 return "sniff_req";
219 case 24:
220 return "unsniff_req";
221 case 25:
222 return "park_req";
223 case 26:
224 return "park";
225 case 27:
226 return "set_broadcast_scan_window";
227 case 28:
228 return "modify_beacon";
229 case 29:
230 return "unpark_BD_ADDR_req";
231 case 30:
232 return "unpark_PM_ADDR_req";
233 case 31:
234 return "incr_power_req";
235 case 32:
236 return "decr_power_req";
237 case 33:
238 return "max_power";
239 case 34:
240 return "min_power";
241 case 35:
242 return "auto_rate";
243 case 36:
244 return "preferred_rate";
245 case 37:
246 return "version_req";
247 case 38:
248 return "version_res";
249 case 39:
250 return "feature_req";
251 case 40:
252 return "feature_res";
253 case 41:
254 return "quality_of_service";
255 case 42:
256 return "quality_of_service_req";
257 case 43:
258 return "SCO_link_req";
259 case 44:
260 return "remove_SCO_link_req";
261 case 45:
262 return "max_slot";
263 case 46:
264 return "max_slot_req";
265 case 47:
266 return "timing_accuracy_req";
267 case 48:
268 return "timing_accuracy_res";
269 case 49:
270 return "setup_complete";
271 case 50:
272 return "use_semi_permanent_key";
273 case 51:
274 return "host_connection_req";
275 case 52:
276 return "slot_offset";
277 case 53:
278 return "page_mode_req";
279 case 54:
280 return "page_scan_mode_req";
281 case 55:
282 return "supervision_timeout";
283 case 56:
284 return "test_activate";
285 case 57:
286 return "test_control";
287 case 58:
288 return "encryption_key_size_mask_req";
289 case 59:
290 return "encryption_key_size_mask_res";
291 case 60:
292 return "set_AFH";
293 case 61:
294 return "encapsulated_header";
295 case 62:
296 return "encapsulated_payload";
297 case 63:
298 return "simple_pairing_confirm";
299 case 64:
300 return "simple_pairing_number";
301 case 65:
302 return "DHkey_check";
303 case 127 + (1 << 7):
304 return "accepted_ext";
305 case 127 + (2 << 7):
306 return "not_accepted_ext";
307 case 127 + (3 << 7):
308 return "features_req_ext";
309 case 127 + (4 << 7):
310 return "features_res_ext";
311 case 127 + (11 << 7):
312 return "packet_type_table_req";
313 case 127 + (12 << 7):
314 return "eSCO_link_req";
315 case 127 + (13 << 7):
316 return "remove_eSCO_link_req";
317 case 127 + (16 << 7):
318 return "channel_classification_req";
319 case 127 + (17 << 7):
320 return "channel_classification";
321 case 127 + (21 << 7):
322 return "sniff_subrating_req";
323 case 127 + (22 << 7):
324 return "sniff_subrating_res";
325 case 127 + (23 << 7):
326 return "pause_encryption_req";
327 case 127 + (24 << 7):
328 return "resume_encryption_req";
329 case 127 + (25 << 7):
330 return "IO_capability_req";
331 case 127 + (26 << 7):
332 return "IO_capability_res";
333 case 127 + (27 << 7):
334 return "numeric_comparison_failed";
335 case 127 + (28 << 7):
336 return "passkey_failed";
337 case 127 + (29 << 7):
338 return "oob_failed";
339 case 127 + (30 << 7):
340 return "keypress_notification";
341 default:
342 return "unknown";
343 }
344 }
345
name_req_dump(int level,struct frame * frm)346 static inline void name_req_dump(int level, struct frame *frm)
347 {
348 uint8_t offset = LMP_U8(frm);
349
350 p_indent(level, frm);
351 printf("name offset %d\n", offset);
352 }
353
name_res_dump(int level,struct frame * frm)354 static inline void name_res_dump(int level, struct frame *frm)
355 {
356 uint8_t offset = LMP_U8(frm);
357 uint8_t length = LMP_U8(frm);
358 uint8_t *name = frm->ptr;
359 int i, size;
360
361 frm->ptr += 14;
362 frm->len -= 14;
363
364 p_indent(level, frm);
365 printf("name offset %d\n", offset);
366
367 p_indent(level, frm);
368 printf("name length %d\n", length);
369
370 size = length - offset;
371 if (size > 14)
372 size = 14;
373
374 p_indent(level, frm);
375 printf("name fragment '");
376 for (i = 0; i < size; i++)
377 if (isprint(name[i]))
378 printf("%c", name[i]);
379 else
380 printf(".");
381 printf("'\n");
382 }
383
accepted_dump(int level,struct frame * frm)384 static inline void accepted_dump(int level, struct frame *frm)
385 {
386 uint8_t opcode = LMP_U8(frm);
387
388 p_indent(level, frm);
389 printf("op code %d (%s)\n", opcode, opcode2str(opcode));
390 }
391
not_accepted_dump(int level,struct frame * frm)392 static inline void not_accepted_dump(int level, struct frame *frm)
393 {
394 uint8_t opcode = LMP_U8(frm);
395 uint8_t error = LMP_U8(frm);
396
397 p_indent(level, frm);
398 printf("op code %d (%s)\n", opcode, opcode2str(opcode));
399
400 p_indent(level, frm);
401 printf("error code 0x%2.2x\n", error);
402 }
403
clkoffset_dump(int level,struct frame * frm)404 static inline void clkoffset_dump(int level, struct frame *frm)
405 {
406 uint16_t clkoffset = LMP_U16(frm);
407
408 p_indent(level, frm);
409 printf("clock offset 0x%4.4x\n", clkoffset);
410 }
411
detach_dump(int level,struct frame * frm)412 static inline void detach_dump(int level, struct frame *frm)
413 {
414 uint8_t error = LMP_U8(frm);
415
416 p_indent(level, frm);
417 printf("error code 0x%2.2x\n", error);
418 }
419
random_number_dump(int level,struct frame * frm)420 static inline void random_number_dump(int level, struct frame *frm)
421 {
422 uint8_t *number = frm->ptr;
423 int i;
424
425 frm->ptr += 16;
426 frm->len -= 16;
427
428 p_indent(level, frm);
429 printf("random number ");
430 for (i = 0; i < 16; i++)
431 printf("%2.2x", number[i]);
432 printf("\n");
433 }
434
key_dump(int level,struct frame * frm)435 static inline void key_dump(int level, struct frame *frm)
436 {
437 uint8_t *key = frm->ptr;
438 int i;
439
440 frm->ptr += 16;
441 frm->len -= 16;
442
443 p_indent(level, frm);
444 printf("key ");
445 for (i = 0; i < 16; i++)
446 printf("%2.2x", key[i]);
447 printf("\n");
448 }
449
auth_resp_dump(int level,struct frame * frm)450 static inline void auth_resp_dump(int level, struct frame *frm)
451 {
452 uint8_t *resp = frm->ptr;
453 int i;
454
455 frm->ptr += 4;
456 frm->ptr -= 4;
457
458 p_indent(level, frm);
459 printf("authentication response ");
460 for (i = 0; i < 4; i++)
461 printf("%2.2x", resp[i]);
462 printf("\n");
463 }
464
encryption_mode_req_dump(int level,struct frame * frm)465 static inline void encryption_mode_req_dump(int level, struct frame *frm)
466 {
467 uint8_t mode = LMP_U8(frm);
468
469 p_indent(level, frm);
470 printf("encryption mode %d\n", mode);
471 }
472
encryption_key_size_req_dump(int level,struct frame * frm)473 static inline void encryption_key_size_req_dump(int level, struct frame *frm)
474 {
475 uint8_t keysize = LMP_U8(frm);
476
477 p_indent(level, frm);
478 printf("key size %d\n", keysize);
479 }
480
switch_req_dump(int level,struct frame * frm)481 static inline void switch_req_dump(int level, struct frame *frm)
482 {
483 uint32_t instant = LMP_U32(frm);
484
485 p_indent(level, frm);
486 printf("switch instant 0x%4.4x\n", instant);
487 }
488
hold_dump(int level,struct frame * frm)489 static inline void hold_dump(int level, struct frame *frm)
490 {
491 uint16_t time = LMP_U16(frm);
492 uint32_t instant = LMP_U32(frm);
493
494 p_indent(level, frm);
495 printf("hold time 0x%4.4x\n", time);
496
497 p_indent(level, frm);
498 printf("hold instant 0x%4.4x\n", instant);
499 }
500
sniff_req_dump(int level,struct frame * frm)501 static inline void sniff_req_dump(int level, struct frame *frm)
502 {
503 uint8_t timing = LMP_U8(frm);
504 uint16_t dsniff = LMP_U16(frm);
505 uint16_t tsniff = LMP_U16(frm);
506 uint16_t attempt = LMP_U16(frm);
507 uint16_t timeout = LMP_U16(frm);
508
509 p_indent(level, frm);
510 printf("timing control flags 0x%2.2x\n", timing);
511
512 p_indent(level, frm);
513 printf("D_sniff %d T_sniff %d\n", dsniff, tsniff);
514
515 p_indent(level, frm);
516 printf("sniff attempt %d\n", attempt);
517
518 p_indent(level, frm);
519 printf("sniff timeout %d\n", timeout);
520 }
521
park_req_dump(int level,struct frame * frm)522 static inline void park_req_dump(int level, struct frame *frm)
523 {
524 uint8_t timing = LMP_U8(frm);
525 uint16_t db = LMP_U16(frm);
526 uint16_t tb = LMP_U16(frm);
527 uint8_t nb = LMP_U8(frm);
528 uint8_t xb = LMP_U8(frm);
529 uint8_t pmaddr = LMP_U8(frm);
530 uint8_t araddr = LMP_U8(frm);
531 uint8_t nbsleep = LMP_U8(frm);
532 uint8_t dbsleep = LMP_U8(frm);
533 uint8_t daccess = LMP_U8(frm);
534 uint8_t taccess = LMP_U8(frm);
535 uint8_t nslots = LMP_U8(frm);
536 uint8_t npoll = LMP_U8(frm);
537 uint8_t access = LMP_U8(frm);
538
539 p_indent(level, frm);
540 printf("timing control flags 0x%2.2x\n", timing);
541
542 p_indent(level, frm);
543 printf("D_B %d T_B %d N_B %d X_B %d\n", db, tb, nb, xb);
544
545 p_indent(level, frm);
546 printf("PM_ADDR %d AR_ADDR %d\n", pmaddr, araddr);
547
548 p_indent(level, frm);
549 printf("N_Bsleep %d D_Bsleep %d\n", nbsleep, dbsleep);
550
551 p_indent(level, frm);
552 printf("D_access %d T_access %d\n", daccess, taccess);
553
554 p_indent(level, frm);
555 printf("N_acc-slots %d N_poll %d\n", nslots, npoll);
556
557 p_indent(level, frm);
558 printf("M_access %d\n", access & 0x0f);
559
560 p_indent(level, frm);
561 printf("access scheme 0x%2.2x\n", access >> 4);
562 }
563
modify_beacon_dump(int level,struct frame * frm)564 static inline void modify_beacon_dump(int level, struct frame *frm)
565 {
566 uint8_t timing = LMP_U8(frm);
567 uint16_t db = LMP_U16(frm);
568 uint16_t tb = LMP_U16(frm);
569 uint8_t nb = LMP_U8(frm);
570 uint8_t xb = LMP_U8(frm);
571 uint8_t daccess = LMP_U8(frm);
572 uint8_t taccess = LMP_U8(frm);
573 uint8_t nslots = LMP_U8(frm);
574 uint8_t npoll = LMP_U8(frm);
575 uint8_t access = LMP_U8(frm);
576
577 p_indent(level, frm);
578 printf("timing control flags 0x%2.2x\n", timing);
579
580 p_indent(level, frm);
581 printf("D_B %d T_B %d N_B %d X_B %d\n", db, tb, nb, xb);
582
583 p_indent(level, frm);
584 printf("D_access %d T_access %d\n", daccess, taccess);
585
586 p_indent(level, frm);
587 printf("N_acc-slots %d N_poll %d\n", nslots, npoll);
588
589 p_indent(level, frm);
590 printf("M_access %d\n", access & 0x0f);
591
592 p_indent(level, frm);
593 printf("access scheme 0x%2.2x\n", access >> 4);
594 }
595
power_req_dump(int level,struct frame * frm)596 static inline void power_req_dump(int level, struct frame *frm)
597 {
598 uint8_t val = LMP_U8(frm);
599
600 p_indent(level, frm);
601 printf("future use 0x%2.2x\n", val);
602 }
603
preferred_rate_dump(int level,struct frame * frm)604 static inline void preferred_rate_dump(int level, struct frame *frm)
605 {
606 uint8_t rate = LMP_U8(frm);
607
608 p_indent(level, frm);
609 printf("data rate 0x%2.2x\n", rate);
610
611 p_indent(level, frm);
612 printf("Basic: ");
613
614 printf("%suse FEC, ", rate & 0x01 ? "do not " : "");
615
616 switch ((rate >> 1) & 0x03) {
617 case 0x00:
618 printf("no packet-size preference\n");
619 break;
620 case 0x01:
621 printf("use 1-slot packets\n");
622 break;
623 case 0x02:
624 printf("use 3-slot packets\n");
625 break;
626 case 0x03:
627 printf("use 5-slot packets\n");
628 break;
629 }
630
631 p_indent(level, frm);
632 printf("EDR: ");
633
634 switch ((rate >> 3) & 0x03) {
635 case 0x00:
636 printf("use DM1 packets, ");
637 break;
638 case 0x01:
639 printf("use 2 Mbps packets, ");
640 break;
641 case 0x02:
642 printf("use 3 Mbps packets, ");
643 break;
644 case 0x03:
645 printf("reserved, \n");
646 break;
647 }
648
649 switch ((rate >> 5) & 0x03) {
650 case 0x00:
651 printf("no packet-size preference\n");
652 break;
653 case 0x01:
654 printf("use 1-slot packets\n");
655 break;
656 case 0x02:
657 printf("use 3-slot packets\n");
658 break;
659 case 0x03:
660 printf("use 5-slot packets\n");
661 break;
662 }
663 }
664
version_dump(int level,struct frame * frm)665 static inline void version_dump(int level, struct frame *frm)
666 {
667 uint8_t ver = LMP_U8(frm);
668 uint16_t compid = LMP_U16(frm);
669 uint16_t subver = LMP_U16(frm);
670 char *tmp;
671
672 p_indent(level, frm);
673 tmp = lmp_vertostr(ver);
674 printf("VersNr %d (%s)\n", ver, tmp);
675 bt_free(tmp);
676
677 p_indent(level, frm);
678 printf("CompId %d (%s)\n", compid, bt_compidtostr(compid));
679
680 p_indent(level, frm);
681 printf("SubVersNr %d\n", subver);
682 }
683
features_dump(int level,struct frame * frm)684 static inline void features_dump(int level, struct frame *frm)
685 {
686 uint8_t *features = frm->ptr;
687 int i;
688
689 frm->ptr += 8;
690 frm->len -= 8;
691
692 p_indent(level, frm);
693 printf("features");
694 for (i = 0; i < 8; i++)
695 printf(" 0x%2.2x", features[i]);
696 printf("\n");
697 }
698
set_afh_dump(int level,struct frame * frm)699 static inline void set_afh_dump(int level, struct frame *frm)
700 {
701 uint32_t instant = LMP_U32(frm);
702 uint8_t mode = LMP_U8(frm);
703 uint8_t *map = frm->ptr;
704 int i;
705
706 frm->ptr += 10;
707 frm->len -= 10;
708
709 p_indent(level, frm);
710 printf("AFH_instant 0x%04x\n", instant);
711
712 p_indent(level, frm);
713 printf("AFH_mode %d\n", mode);
714
715 p_indent(level, frm);
716 printf("AFH_channel_map 0x");
717 for (i = 0; i < 10; i++)
718 printf("%2.2x", map[i]);
719 printf("\n");
720 }
721
encapsulated_header_dump(int level,struct frame * frm)722 static inline void encapsulated_header_dump(int level, struct frame *frm)
723 {
724 uint8_t major = LMP_U8(frm);
725 uint8_t minor = LMP_U8(frm);
726 uint8_t length = LMP_U8(frm);
727
728 p_indent(level, frm);
729 printf("major type %d minor type %d payload length %d\n",
730 major, minor, length);
731
732 if (major == 1 && minor == 1) {
733 p_indent(level, frm);
734 printf("P-192 Public Key\n");
735 }
736 }
737
encapsulated_payload_dump(int level,struct frame * frm)738 static inline void encapsulated_payload_dump(int level, struct frame *frm)
739 {
740 uint8_t *value = frm->ptr;
741 int i;
742
743 frm->ptr += 16;
744 frm->len -= 16;
745
746 p_indent(level, frm);
747 printf("data ");
748 for (i = 0; i < 16; i++)
749 printf("%2.2x", value[i]);
750 printf("\n");
751 }
752
simple_pairing_confirm_dump(int level,struct frame * frm)753 static inline void simple_pairing_confirm_dump(int level, struct frame *frm)
754 {
755 uint8_t *value = frm->ptr;
756 int i;
757
758 frm->ptr += 16;
759 frm->len -= 16;
760
761 p_indent(level, frm);
762 printf("commitment value ");
763 for (i = 0; i < 16; i++)
764 printf("%2.2x", value[i]);
765 printf("\n");
766 }
767
simple_pairing_number_dump(int level,struct frame * frm)768 static inline void simple_pairing_number_dump(int level, struct frame *frm)
769 {
770 uint8_t *value = frm->ptr;
771 int i;
772
773 frm->ptr += 16;
774 frm->len -= 16;
775
776 p_indent(level, frm);
777 printf("nounce value ");
778 for (i = 0; i < 16; i++)
779 printf("%2.2x", value[i]);
780 printf("\n");
781 }
782
dhkey_check_dump(int level,struct frame * frm)783 static inline void dhkey_check_dump(int level, struct frame *frm)
784 {
785 uint8_t *value = frm->ptr;
786 int i;
787
788 frm->ptr += 16;
789 frm->len -= 16;
790
791 p_indent(level, frm);
792 printf("confirmation value ");
793 for (i = 0; i < 16; i++)
794 printf("%2.2x", value[i]);
795 printf("\n");
796 }
797
accepted_ext_dump(int level,struct frame * frm)798 static inline void accepted_ext_dump(int level, struct frame *frm)
799 {
800 uint16_t opcode = LMP_U8(frm) + (LMP_U8(frm) << 7);
801
802 p_indent(level, frm);
803 printf("op code %d/%d (%s)\n", opcode & 0x7f, opcode >> 7, opcode2str(opcode));
804 }
805
not_accepted_ext_dump(int level,struct frame * frm)806 static inline void not_accepted_ext_dump(int level, struct frame *frm)
807 {
808 uint16_t opcode = LMP_U8(frm) + (LMP_U8(frm) << 7);
809 uint8_t error = LMP_U8(frm);
810
811 p_indent(level, frm);
812 printf("op code %d/%d (%s)\n", opcode & 0x7f, opcode >> 7, opcode2str(opcode));
813
814 p_indent(level, frm);
815 printf("error code 0x%2.2x\n", error);
816 }
817
features_ext_dump(int level,struct frame * frm)818 static inline void features_ext_dump(int level, struct frame *frm)
819 {
820 uint8_t page = LMP_U8(frm);
821 uint8_t max = LMP_U8(frm);
822 uint8_t *features = frm->ptr;
823 int i;
824
825 frm->ptr += 8;
826 frm->len -= 8;
827
828 p_indent(level, frm);
829 printf("features page %d\n", page);
830
831 p_indent(level, frm);
832 printf("max supported page %d\n", max);
833
834 p_indent(level, frm);
835 printf("extended features");
836 for (i = 0; i < 8; i++)
837 printf(" 0x%2.2x", features[i]);
838 printf("\n");
839 }
840
quality_of_service_dump(int level,struct frame * frm)841 static inline void quality_of_service_dump(int level, struct frame *frm)
842 {
843 uint16_t interval = LMP_U16(frm);
844 uint8_t nbc = LMP_U8(frm);
845
846 p_indent(level, frm);
847 printf("poll interval %d\n", interval);
848
849 p_indent(level, frm);
850 printf("N_BC %d\n", nbc);
851 }
852
sco_link_req_dump(int level,struct frame * frm)853 static inline void sco_link_req_dump(int level, struct frame *frm)
854 {
855 uint8_t handle = LMP_U8(frm);
856 uint8_t timing = LMP_U8(frm);
857 uint8_t dsco = LMP_U8(frm);
858 uint8_t tsco = LMP_U8(frm);
859 uint8_t packet = LMP_U8(frm);
860 uint8_t airmode = LMP_U8(frm);
861
862 p_indent(level, frm);
863 printf("SCO handle %d\n", handle);
864
865 p_indent(level, frm);
866 printf("timing control flags 0x%2.2x\n", timing);
867
868 p_indent(level, frm);
869 printf("D_SCO %d T_SCO %d\n", dsco, tsco);
870
871 p_indent(level, frm);
872 printf("SCO packet 0x%2.2x\n", packet);
873
874 p_indent(level, frm);
875 printf("air mode 0x%2.2x\n", airmode);
876 }
877
remove_sco_link_req_dump(int level,struct frame * frm)878 static inline void remove_sco_link_req_dump(int level, struct frame *frm)
879 {
880 uint8_t handle = LMP_U8(frm);
881 uint8_t error = LMP_U8(frm);
882
883 p_indent(level, frm);
884 printf("SCO handle %d\n", handle);
885
886 p_indent(level, frm);
887 printf("error code 0x%2.2x\n", error);
888 }
889
max_slots_dump(int level,struct frame * frm)890 static inline void max_slots_dump(int level, struct frame *frm)
891 {
892 uint8_t slots = LMP_U8(frm);
893
894 p_indent(level, frm);
895 printf("max slots %d\n", slots);
896 }
897
timing_accuracy_dump(int level,struct frame * frm)898 static inline void timing_accuracy_dump(int level, struct frame *frm)
899 {
900 uint8_t drift = LMP_U8(frm);
901 uint8_t jitter = LMP_U8(frm);
902
903 p_indent(level, frm);
904 printf("drift %d\n", drift);
905
906 p_indent(level, frm);
907 printf("jitter %d\n", jitter);
908 }
909
slot_offset_dump(int level,struct frame * frm)910 static inline void slot_offset_dump(int level, struct frame *frm)
911 {
912 uint16_t offset = LMP_U16(frm);
913 char addr[18];
914
915 p_ba2str((bdaddr_t *) frm->ptr, addr);
916
917 p_indent(level, frm);
918 printf("slot offset %d\n", offset);
919
920 p_indent(level, frm);
921 printf("BD_ADDR %s\n", addr);
922 }
923
page_mode_dump(int level,struct frame * frm)924 static inline void page_mode_dump(int level, struct frame *frm)
925 {
926 uint8_t scheme = LMP_U8(frm);
927 uint8_t settings = LMP_U8(frm);
928
929 p_indent(level, frm);
930 printf("page scheme %d\n", scheme);
931
932 p_indent(level, frm);
933 printf("page scheme settings %d\n", settings);
934 }
935
supervision_timeout_dump(int level,struct frame * frm)936 static inline void supervision_timeout_dump(int level, struct frame *frm)
937 {
938 uint16_t timeout = LMP_U16(frm);
939
940 p_indent(level, frm);
941 printf("supervision timeout %d\n", timeout);
942 }
943
test_control_dump(int level,struct frame * frm)944 static inline void test_control_dump(int level, struct frame *frm)
945 {
946 uint8_t scenario = LMP_U8(frm);
947 uint8_t hopping = LMP_U8(frm);
948 uint8_t txfreq = LMP_U8(frm);
949 uint8_t rxfreq = LMP_U8(frm);
950 uint8_t power = LMP_U8(frm);
951 uint8_t poll = LMP_U8(frm);
952 uint8_t packet = LMP_U8(frm);
953 uint16_t length = LMP_U16(frm);
954
955 p_indent(level, frm);
956 printf("test scenario %d\n", scenario);
957
958 p_indent(level, frm);
959 printf("hopping mode %d\n", hopping);
960
961 p_indent(level, frm);
962 printf("TX frequency %d\n", txfreq);
963
964 p_indent(level, frm);
965 printf("RX frequency %d\n", rxfreq);
966
967 p_indent(level, frm);
968 printf("power control mode %d\n", power);
969
970 p_indent(level, frm);
971 printf("poll period %d\n", poll);
972
973 p_indent(level, frm);
974 printf("poll period %d\n", poll);
975
976 p_indent(level, frm);
977 printf("packet type 0x%2.2x\n", packet);
978
979 p_indent(level, frm);
980 printf("length of test data %d\n", length);
981 }
982
encryption_key_size_mask_res_dump(int level,struct frame * frm)983 static inline void encryption_key_size_mask_res_dump(int level, struct frame *frm)
984 {
985 uint16_t mask = LMP_U16(frm);
986
987 p_indent(level, frm);
988 printf("key size mask 0x%4.4x\n", mask);
989 }
990
packet_type_table_dump(int level,struct frame * frm)991 static inline void packet_type_table_dump(int level, struct frame *frm)
992 {
993 uint8_t type = LMP_U8(frm);
994
995 p_indent(level, frm);
996 printf("packet type table %d ", type);
997 switch (type) {
998 case 0:
999 printf("(1Mbps only)\n");
1000 break;
1001 case 1:
1002 printf("(2/3Mbps)\n");
1003 break;
1004 default:
1005 printf("(Reserved)\n");
1006 break;
1007 }
1008 }
1009
esco_link_req_dump(int level,struct frame * frm)1010 static inline void esco_link_req_dump(int level, struct frame *frm)
1011 {
1012 uint8_t handle = LMP_U8(frm);
1013 uint8_t ltaddr = LMP_U8(frm);
1014 uint8_t timing = LMP_U8(frm);
1015 uint8_t desco = LMP_U8(frm);
1016 uint8_t tesco = LMP_U8(frm);
1017 uint8_t wesco = LMP_U8(frm);
1018 uint8_t mspkt = LMP_U8(frm);
1019 uint8_t smpkt = LMP_U8(frm);
1020 uint16_t mslen = LMP_U16(frm);
1021 uint16_t smlen = LMP_U16(frm);
1022 uint8_t airmode = LMP_U8(frm);
1023 uint8_t negstate = LMP_U8(frm);
1024
1025 p_indent(level, frm);
1026 printf("eSCO handle %d\n", handle);
1027
1028 p_indent(level, frm);
1029 printf("eSCO LT_ADDR %d\n", ltaddr);
1030
1031 p_indent(level, frm);
1032 printf("timing control flags 0x%2.2x\n", timing);
1033
1034 p_indent(level, frm);
1035 printf("D_eSCO %d T_eSCO %d W_eSCO %d\n", desco, tesco, wesco);
1036
1037 p_indent(level, frm);
1038 printf("eSCO M->S packet type 0x%2.2x length %d\n", mspkt, mslen);
1039
1040 p_indent(level, frm);
1041 printf("eSCO S->M packet type 0x%2.2x length %d\n", smpkt, smlen);
1042
1043 p_indent(level, frm);
1044 printf("air mode 0x%2.2x\n", airmode);
1045
1046 p_indent(level, frm);
1047 printf("negotiation state 0x%2.2x\n", negstate);
1048 }
1049
remove_esco_link_req_dump(int level,struct frame * frm)1050 static inline void remove_esco_link_req_dump(int level, struct frame *frm)
1051 {
1052 uint8_t handle = LMP_U8(frm);
1053 uint8_t error = LMP_U8(frm);
1054
1055 p_indent(level, frm);
1056 printf("eSCO handle %d\n", handle);
1057
1058 p_indent(level, frm);
1059 printf("error code 0x%2.2x\n", error);
1060 }
1061
channel_classification_req_dump(int level,struct frame * frm)1062 static inline void channel_classification_req_dump(int level, struct frame *frm)
1063 {
1064 uint8_t mode = LMP_U8(frm);
1065 uint16_t min = LMP_U16(frm);
1066 uint16_t max = LMP_U16(frm);
1067
1068 p_indent(level, frm);
1069 printf("AFH reporting mode %d\n", mode);
1070
1071 p_indent(level, frm);
1072 printf("AFH min interval 0x%4.4x\n", min);
1073
1074 p_indent(level, frm);
1075 printf("AFH max interval 0x%4.4x\n", max);
1076 }
1077
channel_classification_dump(int level,struct frame * frm)1078 static inline void channel_classification_dump(int level, struct frame *frm)
1079 {
1080 uint8_t *map = frm->ptr;
1081 int i;
1082
1083 frm->ptr += 10;
1084 frm->len -= 10;
1085
1086 p_indent(level, frm);
1087 printf("AFH channel classification 0x");
1088 for (i = 0; i < 10; i++)
1089 printf("%2.2x", map[i]);
1090 printf("\n");
1091 }
1092
sniff_subrating_dump(int level,struct frame * frm)1093 static inline void sniff_subrating_dump(int level, struct frame *frm)
1094 {
1095 uint8_t subrate = LMP_U8(frm);
1096 uint16_t timeout = LMP_U16(frm);
1097 uint32_t instant = LMP_U32(frm);
1098
1099 p_indent(level, frm);
1100 printf("max subrate %d\n", subrate);
1101
1102 p_indent(level, frm);
1103 printf("min sniff timeout %d\n", timeout);
1104
1105 p_indent(level, frm);
1106 printf("subrate instant 0x%4.4x\n", instant);
1107 }
1108
io_capability_dump(int level,struct frame * frm)1109 static inline void io_capability_dump(int level, struct frame *frm)
1110 {
1111 uint8_t capability = LMP_U8(frm);
1112 uint8_t oob_data = LMP_U8(frm);
1113 uint8_t authentication = LMP_U8(frm);
1114
1115 p_indent(level, frm);
1116 printf("capability 0x%2.2x oob 0x%2.2x auth 0x%2.2x\n",
1117 capability, oob_data, authentication);
1118 }
1119
keypress_notification_dump(int level,struct frame * frm)1120 static inline void keypress_notification_dump(int level, struct frame *frm)
1121 {
1122 uint8_t value = LMP_U8(frm);
1123
1124 p_indent(level, frm);
1125 printf("notification value %d\n", value);
1126 }
1127
lmp_dump(int level,struct frame * frm)1128 void lmp_dump(int level, struct frame *frm)
1129 {
1130 uint8_t tmp, tid;
1131 uint16_t opcode;
1132
1133 p_indent(level, frm);
1134
1135 tmp = LMP_U8(frm);
1136 tid = tmp & 0x01;
1137 opcode = (tmp & 0xfe) >> 1;
1138 if (opcode > 123) {
1139 tmp = LMP_U8(frm);
1140 opcode += tmp << 7;
1141 }
1142
1143 printf("LMP(%c): %s(%c): ", frm->master ? 's' : 'r',
1144 opcode2str(opcode), tid ? 's' : 'm');
1145
1146 if (opcode > 123)
1147 printf("op code %d/%d", opcode & 0x7f, opcode >> 7);
1148 else
1149 printf("op code %d", opcode);
1150
1151 if (frm->handle > 17)
1152 printf(" handle %d\n", frm->handle);
1153 else
1154 printf("\n");
1155
1156 if (!(parser.flags & DUMP_VERBOSE)) {
1157 raw_dump(level, frm);
1158 return;
1159 }
1160
1161 switch (opcode) {
1162 case 1:
1163 name_req_dump(level + 1, frm);
1164 return;
1165 case 2:
1166 name_res_dump(level + 1, frm);
1167 return;
1168 case 3:
1169 accepted_dump(level + 1, frm);
1170 return;
1171 case 4:
1172 not_accepted_dump(level + 1, frm);
1173 return;
1174 case 6:
1175 clkoffset_dump(level + 1, frm);
1176 return;
1177 case 7:
1178 detach_dump(level + 1, frm);
1179 return;
1180 case 8:
1181 in_rand(frm);
1182 random_number_dump(level + 1, frm);
1183 return;
1184 case 9:
1185 comb_key(frm);
1186 random_number_dump(level + 1, frm);
1187 return;
1188 case 11:
1189 au_rand(frm);
1190 random_number_dump(level + 1, frm);
1191 return;
1192 case 12:
1193 sres(frm);
1194 auth_resp_dump(level + 1, frm);
1195 return;
1196 case 13:
1197 case 17:
1198 random_number_dump(level + 1, frm);
1199 return;
1200 case 10:
1201 case 14:
1202 key_dump(level + 1, frm);
1203 return;
1204 case 15:
1205 encryption_mode_req_dump(level + 1, frm);
1206 return;
1207 case 16:
1208 encryption_key_size_req_dump(level + 1, frm);
1209 return;
1210 case 19:
1211 switch_req_dump(level + 1, frm);
1212 return;
1213 case 20:
1214 case 21:
1215 hold_dump(level + 1, frm);
1216 return;
1217 case 23:
1218 sniff_req_dump(level + 1, frm);
1219 return;
1220 case 25:
1221 park_req_dump(level + 1, frm);
1222 return;
1223 case 28:
1224 modify_beacon_dump(level + 1, frm);
1225 return;
1226 case 31:
1227 case 32:
1228 power_req_dump(level + 1, frm);
1229 return;
1230 case 36:
1231 preferred_rate_dump(level + 1, frm);
1232 return;
1233 case 37:
1234 case 38:
1235 version_dump(level + 1, frm);
1236 return;
1237 case 39:
1238 case 40:
1239 features_dump(level + 1, frm);
1240 return;
1241 case 41:
1242 case 42:
1243 quality_of_service_dump(level + 1, frm);
1244 return;
1245 case 43:
1246 sco_link_req_dump(level + 1, frm);
1247 return;
1248 case 44:
1249 remove_sco_link_req_dump(level + 1, frm);
1250 return;
1251 case 45:
1252 case 46:
1253 max_slots_dump(level + 1, frm);
1254 return;
1255 case 48:
1256 timing_accuracy_dump(level + 1, frm);
1257 return;
1258 case 52:
1259 slot_offset_dump(level + 1, frm);
1260 return;
1261 case 53:
1262 case 54:
1263 page_mode_dump(level + 1, frm);
1264 return;
1265 case 55:
1266 supervision_timeout_dump(level + 1, frm);
1267 return;
1268 case 57:
1269 test_control_dump(level + 1, frm);
1270 return;
1271 case 59:
1272 encryption_key_size_mask_res_dump(level + 1, frm);
1273 return;
1274 case 60:
1275 set_afh_dump(level + 1, frm);
1276 return;
1277 case 61:
1278 encapsulated_header_dump(level + 1, frm);
1279 return;
1280 case 62:
1281 encapsulated_payload_dump(level + 1, frm);
1282 return;
1283 case 63:
1284 simple_pairing_confirm_dump(level + 1, frm);
1285 return;
1286 case 64:
1287 simple_pairing_number_dump(level + 1, frm);
1288 return;
1289 case 65:
1290 dhkey_check_dump(level + 1, frm);
1291 return;
1292 case 5:
1293 case 18:
1294 case 24:
1295 case 33:
1296 case 34:
1297 case 35:
1298 case 47:
1299 case 49:
1300 case 50:
1301 case 51:
1302 case 56:
1303 case 58:
1304 case 127 + (23 << 7):
1305 case 127 + (24 << 7):
1306 case 127 + (27 << 7):
1307 case 127 + (28 << 7):
1308 case 127 + (29 << 7):
1309 return;
1310 case 127 + (1 << 7):
1311 accepted_ext_dump(level + 1, frm);
1312 return;
1313 case 127 + (2 << 7):
1314 not_accepted_ext_dump(level + 1, frm);
1315 return;
1316 case 127 + (3 << 7):
1317 case 127 + (4 << 7):
1318 features_ext_dump(level + 1, frm);
1319 return;
1320 case 127 + (11 << 7):
1321 packet_type_table_dump(level + 1, frm);
1322 return;
1323 case 127 + (12 << 7):
1324 esco_link_req_dump(level + 1, frm);
1325 return;
1326 case 127 + (13 << 7):
1327 remove_esco_link_req_dump(level + 1, frm);
1328 return;
1329 case 127 + (16 << 7):
1330 channel_classification_req_dump(level + 1, frm);
1331 return;
1332 case 127 + (17 << 7):
1333 channel_classification_dump(level + 1, frm);
1334 return;
1335 case 127 + (21 << 7):
1336 case 127 + (22 << 7):
1337 sniff_subrating_dump(level + 1, frm);
1338 return;
1339 case 127 + (25 << 7):
1340 case 127 + (26 << 7):
1341 io_capability_dump(level + 1, frm);
1342 return;
1343 case 127 + (30 << 7):
1344 keypress_notification_dump(level + 1, frm);
1345 return;
1346 }
1347
1348 raw_dump(level, frm);
1349 }
1350