1 /*
2 *
3 * BlueZ - Bluetooth protocol stack for Linux
4 *
5 * Copyright (C) 2000-2002 Maxim Krasnyansky <maxk@qualcomm.com>
6 * Copyright (C) 2003-2011 Marcel Holtmann <marcel@holtmann.org>
7 *
8 *
9 * This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU General Public License for more details.
18 *
19 * You should have received a copy of the GNU General Public License
20 * along with this program; if not, write to the Free Software
21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
22 *
23 */
24
25 #ifdef HAVE_CONFIG_H
26 #include <config.h>
27 #endif
28
29 #include <stdio.h>
30 #include <errno.h>
31 #include <unistd.h>
32 #include <stdlib.h>
33 #include <string.h>
34
35 #include <sys/types.h>
36 #include <sys/socket.h>
37 #include <netinet/in.h>
38
39 #include <bluetooth/bluetooth.h>
40 #include <bluetooth/hci.h>
41 #include <bluetooth/l2cap.h>
42
43 #include "parser.h"
44 #include "sdp.h"
45
46 typedef struct {
47 uint16_t handle;
48 struct frame frm;
49 } handle_info;
50 #define HANDLE_TABLE_SIZE 10
51
52 static handle_info handle_table[HANDLE_TABLE_SIZE];
53
54 typedef struct {
55 uint16_t handle;
56 uint16_t cid;
57 uint16_t psm;
58 uint16_t num;
59 uint8_t mode;
60 } cid_info;
61 #define CID_TABLE_SIZE 20
62
63 static cid_info cid_table[2][CID_TABLE_SIZE];
64
65 #define SCID cid_table[0]
66 #define DCID cid_table[1]
67
add_handle(uint16_t handle)68 static struct frame *add_handle(uint16_t handle)
69 {
70 register handle_info *t = handle_table;
71 register int i;
72
73 for (i = 0; i < HANDLE_TABLE_SIZE; i++)
74 if (!t[i].handle) {
75 t[i].handle = handle;
76 return &t[i].frm;
77 }
78 return NULL;
79 }
80
get_frame(uint16_t handle)81 static struct frame *get_frame(uint16_t handle)
82 {
83 register handle_info *t = handle_table;
84 register int i;
85
86 for (i = 0; i < HANDLE_TABLE_SIZE; i++)
87 if (t[i].handle == handle)
88 return &t[i].frm;
89
90 return add_handle(handle);
91 }
92
add_cid(int in,uint16_t handle,uint16_t cid,uint16_t psm)93 static void add_cid(int in, uint16_t handle, uint16_t cid, uint16_t psm)
94 {
95 register cid_info *table = cid_table[in];
96 register int i, pos = -1;
97 uint16_t num = 1;
98
99 for (i = 0; i < CID_TABLE_SIZE; i++) {
100 if ((pos < 0 && !table[i].cid) || table[i].cid == cid)
101 pos = i;
102 if (table[i].psm == psm)
103 num++;
104 }
105
106 if (pos >= 0) {
107 table[pos].handle = handle;
108 table[pos].cid = cid;
109 table[pos].psm = psm;
110 table[pos].num = num;
111 table[pos].mode = 0;
112 }
113 }
114
del_cid(int in,uint16_t dcid,uint16_t scid)115 static void del_cid(int in, uint16_t dcid, uint16_t scid)
116 {
117 register int t, i;
118 uint16_t cid[2];
119
120 if (!in) {
121 cid[0] = dcid;
122 cid[1] = scid;
123 } else {
124 cid[0] = scid;
125 cid[1] = dcid;
126 }
127
128 for (t = 0; t < 2; t++) {
129 for (i = 0; i < CID_TABLE_SIZE; i++)
130 if (cid_table[t][i].cid == cid[t]) {
131 cid_table[t][i].handle = 0;
132 cid_table[t][i].cid = 0;
133 cid_table[t][i].psm = 0;
134 cid_table[t][i].num = 0;
135 cid_table[t][i].mode = 0;
136 break;
137 }
138 }
139 }
140
del_handle(uint16_t handle)141 static void del_handle(uint16_t handle)
142 {
143 register int t, i;
144
145 for (t = 0; t < 2; t++) {
146 for (i = 0; i < CID_TABLE_SIZE; i++)
147 if (cid_table[t][i].handle == handle) {
148 cid_table[t][i].handle = 0;
149 cid_table[t][i].cid = 0;
150 cid_table[t][i].psm = 0;
151 cid_table[t][i].num = 0;
152 cid_table[t][i].mode = 0;
153 break;
154 }
155 }
156 }
get_psm(int in,uint16_t cid)157 static uint16_t get_psm(int in, uint16_t cid)
158 {
159 register cid_info *table = cid_table[in];
160 register int i;
161
162 for (i = 0; i < CID_TABLE_SIZE; i++)
163 if (table[i].cid == cid)
164 return table[i].psm;
165 return parser.defpsm;
166 }
167
get_num(int in,uint16_t cid)168 static uint16_t get_num(int in, uint16_t cid)
169 {
170 register cid_info *table = cid_table[in];
171 register int i;
172
173 for (i = 0; i < CID_TABLE_SIZE; i++)
174 if (table[i].cid == cid)
175 return table[i].num;
176 return 0;
177 }
178
set_mode(int in,uint16_t cid,uint8_t mode)179 static void set_mode(int in, uint16_t cid, uint8_t mode)
180 {
181 register cid_info *table = cid_table[in];
182 register int i;
183
184 for (i = 0; i < CID_TABLE_SIZE; i++)
185 if (table[i].cid == cid)
186 table[i].mode = mode;
187 }
188
get_mode(int in,uint16_t cid)189 static uint8_t get_mode(int in, uint16_t cid)
190 {
191 register cid_info *table = cid_table[in];
192 register int i;
193
194 for (i = 0; i < CID_TABLE_SIZE; i++)
195 if (table[i].cid == cid)
196 return table[i].mode;
197 return 0;
198 }
199
get_val(uint8_t * ptr,uint8_t len)200 static uint32_t get_val(uint8_t *ptr, uint8_t len)
201 {
202 switch (len) {
203 case 1:
204 return *ptr;
205 case 2:
206 return btohs(bt_get_unaligned((uint16_t *) ptr));
207 case 4:
208 return btohl(bt_get_unaligned((uint32_t *) ptr));
209 }
210 return 0;
211 }
212
reason2str(uint16_t reason)213 static char *reason2str(uint16_t reason)
214 {
215 switch (reason) {
216 case 0x0000:
217 return "Command not understood";
218 case 0x0001:
219 return "Signalling MTU exceeded";
220 case 0x0002:
221 return "Invalid CID in request";
222 default:
223 return "Reserved";
224 }
225 }
226
connresult2str(uint16_t result)227 static char *connresult2str(uint16_t result)
228 {
229 switch (result) {
230 case 0x0000:
231 return "Connection successful";
232 case 0x0001:
233 return "Connection pending";
234 case 0x0002:
235 return "Connection refused - PSM not supported";
236 case 0x0003:
237 return "Connection refused - security block";
238 case 0x0004:
239 return "Connection refused - no resources available";
240 default:
241 return "Reserved";
242 }
243 }
244
status2str(uint16_t status)245 static char *status2str(uint16_t status)
246 {
247 switch (status) {
248 case 0x0000:
249 return "No futher information available";
250 case 0x0001:
251 return "Authentication pending";
252 case 0x0002:
253 return "Authorization pending";
254 default:
255 return "Reserved";
256 }
257 }
258
confresult2str(uint16_t result)259 static char *confresult2str(uint16_t result)
260 {
261 switch (result) {
262 case 0x0000:
263 return "Success";
264 case 0x0001:
265 return "Failure - unacceptable parameters";
266 case 0x0002:
267 return "Failure - rejected (no reason provided)";
268 case 0x0003:
269 return "Failure - unknown options";
270 default:
271 return "Reserved";
272 }
273 }
inforesult2str(uint16_t result)274 static char *inforesult2str(uint16_t result)
275 {
276 switch (result) {
277 case 0x0000:
278 return "Success";
279 case 0x0001:
280 return "Not supported";
281 default:
282 return "Reserved";
283 }
284 }
285
type2str(uint8_t type)286 static char *type2str(uint8_t type)
287 {
288 switch (type) {
289 case 0x00:
290 return "No traffic";
291 case 0x01:
292 return "Best effort";
293 case 0x02:
294 return "Guaranteed";
295 default:
296 return "Reserved";
297 }
298 }
299
mode2str(uint8_t mode)300 static char *mode2str(uint8_t mode)
301 {
302 switch (mode) {
303 case 0x00:
304 return "Basic";
305 case 0x01:
306 return "Retransmission";
307 case 0x02:
308 return "Flow control";
309 case 0x03:
310 return "Enhanced Retransmission";
311 case 0x04:
312 return "Streaming";
313 default:
314 return "Reserved";
315 }
316 }
317
fcs2str(uint8_t fcs)318 static char *fcs2str(uint8_t fcs)
319 {
320 switch (fcs) {
321 case 0x00:
322 return "No FCS";
323 case 0x01:
324 return "CRC16 Check";
325 default:
326 return "Reserved";
327 }
328 }
329
sar2str(uint8_t sar)330 static char *sar2str(uint8_t sar)
331 {
332 switch (sar) {
333 case 0x00:
334 return "Unsegmented";
335 case 0x01:
336 return "Start";
337 case 0x02:
338 return "End";
339 case 0x03:
340 return "Continuation";
341 default:
342 return "Bad SAR";
343
344 }
345 }
346
supervisory2str(uint8_t supervisory)347 static char *supervisory2str(uint8_t supervisory)
348 {
349 switch (supervisory) {
350 case 0x00:
351 return "Receiver Ready (RR)";
352 case 0x01:
353 return "Reject (REJ)";
354 case 0x02:
355 return "Receiver Not Ready (RNR)";
356 case 0x03:
357 return "Select Reject (SREJ)";
358 default:
359 return "Bad Supervisory";
360 }
361 }
362
command_rej(int level,struct frame * frm)363 static inline void command_rej(int level, struct frame *frm)
364 {
365 l2cap_cmd_rej *h = frm->ptr;
366 uint16_t reason = btohs(h->reason);
367 uint32_t cid;
368
369 printf("Command rej: reason %d", reason);
370
371 switch (reason) {
372 case 0x0001:
373 printf(" mtu %d\n", get_val(frm->ptr + L2CAP_CMD_REJ_SIZE, 2));
374 break;
375 case 0x0002:
376 cid = get_val(frm->ptr + L2CAP_CMD_REJ_SIZE, 4);
377 printf(" dcid 0x%4.4x scid 0x%4.4x\n", cid & 0xffff, cid >> 16);
378 break;
379 default:
380 printf("\n");
381 break;
382 }
383
384 p_indent(level + 1, frm);
385 printf("%s\n", reason2str(reason));
386 }
387
conn_req(int level,struct frame * frm)388 static inline void conn_req(int level, struct frame *frm)
389 {
390 l2cap_conn_req *h = frm->ptr;
391 uint16_t psm = btohs(h->psm);
392 uint16_t scid = btohs(h->scid);
393
394 add_cid(frm->in, frm->handle, scid, psm);
395
396 if (p_filter(FILT_L2CAP))
397 return;
398
399 printf("Connect req: psm %d scid 0x%4.4x\n", psm, scid);
400 }
401
conn_rsp(int level,struct frame * frm)402 static inline void conn_rsp(int level, struct frame *frm)
403 {
404 l2cap_conn_rsp *h = frm->ptr;
405 uint16_t scid = btohs(h->scid);
406 uint16_t dcid = btohs(h->dcid);
407 uint16_t result = btohs(h->result);
408 uint16_t status = btohs(h->status);
409 uint16_t psm;
410
411 switch (h->result) {
412 case L2CAP_CR_SUCCESS:
413 if ((psm = get_psm(!frm->in, scid)))
414 add_cid(frm->in, frm->handle, dcid, psm);
415 break;
416
417 case L2CAP_CR_PEND:
418 break;
419
420 default:
421 del_cid(frm->in, dcid, scid);
422 break;
423 }
424
425 if (p_filter(FILT_L2CAP))
426 return;
427
428 printf("Connect rsp: dcid 0x%4.4x scid 0x%4.4x result %d status %d\n",
429 dcid, scid, result, status);
430
431 p_indent(level + 1, frm);
432 printf("%s", connresult2str(result));
433
434 if (result == 0x0001)
435 printf(" - %s\n", status2str(status));
436 else
437 printf("\n");
438 }
439
conf_rfc(void * ptr,int len,int in,uint16_t cid)440 static void conf_rfc(void *ptr, int len, int in, uint16_t cid)
441 {
442 uint8_t mode;
443
444 mode = *((uint8_t *) ptr);
445 set_mode(in, cid, mode);
446
447 printf("RFC 0x%02x (%s", mode, mode2str(mode));
448 if (mode >= 0x01 && mode <= 0x04) {
449 uint8_t txwin, maxtrans;
450 uint16_t rto, mto, mps;
451 txwin = *((uint8_t *) (ptr + 1));
452 maxtrans = *((uint8_t *) (ptr + 2));
453 rto = btohs(bt_get_unaligned((uint16_t *) (ptr + 3)));
454 mto = btohs(bt_get_unaligned((uint16_t *) (ptr + 5)));
455 mps = btohs(bt_get_unaligned((uint16_t *) (ptr + 7)));
456 printf(", TxWin %d, MaxTx %d, RTo %d, MTo %d, MPS %d",
457 txwin, maxtrans, rto, mto, mps);
458 }
459 printf(")");
460 }
461
conf_fcs(void * ptr,int len)462 static void conf_fcs(void *ptr, int len)
463 {
464 uint8_t fcs;
465
466 fcs = *((uint8_t *) ptr);
467 printf("FCS Option");
468 if (len > 0)
469 printf(" 0x%2.2x (%s)", fcs, fcs2str(fcs));
470 }
471
conf_opt(int level,void * ptr,int len,int in,uint16_t cid)472 static void conf_opt(int level, void *ptr, int len, int in, uint16_t cid)
473 {
474 p_indent(level, 0);
475 while (len > 0) {
476 l2cap_conf_opt *h = ptr;
477
478 ptr += L2CAP_CONF_OPT_SIZE + h->len;
479 len -= L2CAP_CONF_OPT_SIZE + h->len;
480
481 if (h->type & 0x80)
482 printf("[");
483
484 switch (h->type & 0x7f) {
485 case L2CAP_CONF_MTU:
486 set_mode(in, cid, 0x00);
487 printf("MTU");
488 if (h->len > 0)
489 printf(" %d", get_val(h->val, h->len));
490 break;
491
492 case L2CAP_CONF_FLUSH_TO:
493 printf("FlushTO");
494 if (h->len > 0)
495 printf(" %d", get_val(h->val, h->len));
496 break;
497
498 case L2CAP_CONF_QOS:
499 printf("QoS");
500 if (h->len > 0)
501 printf(" 0x%02x (%s)", *(h->val + 1), type2str(*(h->val + 1)));
502 break;
503
504 case L2CAP_CONF_RFC:
505 conf_rfc(h->val, h->len, in, cid);
506 break;
507
508 case L2CAP_CONF_FCS:
509 conf_fcs(h->val, h->len);
510 break;
511
512 default:
513 printf("Unknown (type %2.2x, len %d)", h->type & 0x7f, h->len);
514 break;
515 }
516
517 if (h->type & 0x80)
518 printf("] ");
519 else
520 printf(" ");
521 }
522 printf("\n");
523 }
524
conf_list(int level,uint8_t * list,int len)525 static void conf_list(int level, uint8_t *list, int len)
526 {
527 int i;
528
529 p_indent(level, 0);
530 for (i = 0; i < len; i++) {
531 switch (list[i] & 0x7f) {
532 case L2CAP_CONF_MTU:
533 printf("MTU ");
534 break;
535 case L2CAP_CONF_FLUSH_TO:
536 printf("FlushTo ");
537 break;
538 case L2CAP_CONF_QOS:
539 printf("QoS ");
540 break;
541 case L2CAP_CONF_RFC:
542 printf("RFC ");
543 break;
544 case L2CAP_CONF_FCS:
545 printf("FCS ");
546 break;
547 default:
548 printf("%2.2x ", list[i] & 0x7f);
549 break;
550 }
551 }
552 printf("\n");
553 }
554
conf_req(int level,l2cap_cmd_hdr * cmd,struct frame * frm)555 static inline void conf_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
556 {
557 l2cap_conf_req *h = frm->ptr;
558 uint16_t dcid = btohs(h->dcid);
559 int clen = btohs(cmd->len) - L2CAP_CONF_REQ_SIZE;
560
561 if (p_filter(FILT_L2CAP))
562 return;
563
564 printf("Config req: dcid 0x%4.4x flags 0x%2.2x clen %d\n",
565 dcid, btohs(h->flags), clen);
566
567 if (clen > 0)
568 conf_opt(level + 1, h->data, clen, frm->in, dcid);
569 }
570
conf_rsp(int level,l2cap_cmd_hdr * cmd,struct frame * frm)571 static inline void conf_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
572 {
573 l2cap_conf_rsp *h = frm->ptr;
574 uint16_t scid = btohs(h->scid);
575 uint16_t result = btohs(h->result);
576 int clen = btohs(cmd->len) - L2CAP_CONF_RSP_SIZE;
577
578 if (p_filter(FILT_L2CAP))
579 return;
580
581 printf("Config rsp: scid 0x%4.4x flags 0x%2.2x result %d clen %d\n",
582 scid, btohs(h->flags), result, clen);
583
584 if (clen > 0) {
585 if (result) {
586 p_indent(level + 1, frm);
587 printf("%s\n", confresult2str(result));
588 }
589 if (result == 0x0003)
590 conf_list(level + 1, h->data, clen);
591 else
592 conf_opt(level + 1, h->data, clen, frm->in, scid);
593 } else {
594 p_indent(level + 1, frm);
595 printf("%s\n", confresult2str(result));
596 }
597 }
598
disconn_req(int level,struct frame * frm)599 static inline void disconn_req(int level, struct frame *frm)
600 {
601 l2cap_disconn_req *h = frm->ptr;
602
603 if (p_filter(FILT_L2CAP))
604 return;
605
606 printf("Disconn req: dcid 0x%4.4x scid 0x%4.4x\n",
607 btohs(h->dcid), btohs(h->scid));
608 }
609
disconn_rsp(int level,struct frame * frm)610 static inline void disconn_rsp(int level, struct frame *frm)
611 {
612 l2cap_disconn_rsp *h = frm->ptr;
613 uint16_t dcid = btohs(h->dcid);
614 uint16_t scid = btohs(h->scid);
615
616 del_cid(frm->in, dcid, scid);
617
618 if (p_filter(FILT_L2CAP))
619 return;
620
621 printf("Disconn rsp: dcid 0x%4.4x scid 0x%4.4x\n",
622 btohs(h->dcid), btohs(h->scid));
623 }
624
echo_req(int level,l2cap_cmd_hdr * cmd,struct frame * frm)625 static inline void echo_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
626 {
627 if (p_filter(FILT_L2CAP))
628 return;
629
630 printf("Echo req: dlen %d\n", btohs(cmd->len));
631 raw_dump(level, frm);
632 }
633
echo_rsp(int level,l2cap_cmd_hdr * cmd,struct frame * frm)634 static inline void echo_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
635 {
636 if (p_filter(FILT_L2CAP))
637 return;
638
639 printf("Echo rsp: dlen %d\n", btohs(cmd->len));
640 raw_dump(level, frm);
641 }
642
info_opt(int level,int type,void * ptr,int len)643 static void info_opt(int level, int type, void *ptr, int len)
644 {
645 uint32_t mask;
646
647 p_indent(level, 0);
648
649 switch (type) {
650 case 0x0001:
651 printf("Connectionless MTU %d\n", get_val(ptr, len));
652 break;
653 case 0x0002:
654 mask = get_val(ptr, len);
655 printf("Extended feature mask 0x%4.4x\n", mask);
656 if (parser.flags & DUMP_VERBOSE) {
657 if (mask & 0x01) {
658 p_indent(level + 1, 0);
659 printf("Flow control mode\n");
660 }
661 if (mask & 0x02) {
662 p_indent(level + 1, 0);
663 printf("Retransmission mode\n");
664 }
665 if (mask & 0x04) {
666 p_indent(level + 1, 0);
667 printf("Bi-directional QoS\n");
668 }
669 }
670 break;
671 case 0x0003:
672 printf("Fixed channel list\n");
673 break;
674 default:
675 printf("Unknown (len %d)\n", len);
676 break;
677 }
678 }
679
info_req(int level,l2cap_cmd_hdr * cmd,struct frame * frm)680 static inline void info_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
681 {
682 l2cap_info_req *h = frm->ptr;
683
684 if (p_filter(FILT_L2CAP))
685 return;
686
687 printf("Info req: type %d\n", btohs(h->type));
688 }
689
info_rsp(int level,l2cap_cmd_hdr * cmd,struct frame * frm)690 static inline void info_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
691 {
692 l2cap_info_rsp *h = frm->ptr;
693 uint16_t type = btohs(h->type);
694 uint16_t result = btohs(h->result);
695 int ilen = btohs(cmd->len) - L2CAP_INFO_RSP_SIZE;
696
697 if (p_filter(FILT_L2CAP))
698 return;
699
700 printf("Info rsp: type %d result %d\n", type, result);
701
702 if (ilen > 0) {
703 info_opt(level + 1, type, h->data, ilen);
704 } else {
705 p_indent(level + 1, frm);
706 printf("%s\n", inforesult2str(result));
707 }
708 }
709
l2cap_parse(int level,struct frame * frm)710 static void l2cap_parse(int level, struct frame *frm)
711 {
712 l2cap_hdr *hdr = (void *)frm->ptr;
713 uint16_t dlen = btohs(hdr->len);
714 uint16_t cid = btohs(hdr->cid);
715 uint16_t psm;
716
717 frm->ptr += L2CAP_HDR_SIZE;
718 frm->len -= L2CAP_HDR_SIZE;
719
720 if (cid == 0x1) {
721 /* Signaling channel */
722
723 while (frm->len >= L2CAP_CMD_HDR_SIZE) {
724 l2cap_cmd_hdr *hdr = frm->ptr;
725
726 frm->ptr += L2CAP_CMD_HDR_SIZE;
727 frm->len -= L2CAP_CMD_HDR_SIZE;
728
729 if (!p_filter(FILT_L2CAP)) {
730 p_indent(level, frm);
731 printf("L2CAP(s): ");
732 }
733
734 switch (hdr->code) {
735 case L2CAP_COMMAND_REJ:
736 command_rej(level, frm);
737 break;
738
739 case L2CAP_CONN_REQ:
740 conn_req(level, frm);
741 break;
742
743 case L2CAP_CONN_RSP:
744 conn_rsp(level, frm);
745 break;
746
747 case L2CAP_CONF_REQ:
748 conf_req(level, hdr, frm);
749 break;
750
751 case L2CAP_CONF_RSP:
752 conf_rsp(level, hdr, frm);
753 break;
754
755 case L2CAP_DISCONN_REQ:
756 disconn_req(level, frm);
757 break;
758
759 case L2CAP_DISCONN_RSP:
760 disconn_rsp(level, frm);
761 break;
762
763 case L2CAP_ECHO_REQ:
764 echo_req(level, hdr, frm);
765 break;
766
767 case L2CAP_ECHO_RSP:
768 echo_rsp(level, hdr, frm);
769 break;
770
771 case L2CAP_INFO_REQ:
772 info_req(level, hdr, frm);
773 break;
774
775 case L2CAP_INFO_RSP:
776 info_rsp(level, hdr, frm);
777 break;
778
779 default:
780 if (p_filter(FILT_L2CAP))
781 break;
782 printf("code 0x%2.2x ident %d len %d\n",
783 hdr->code, hdr->ident, btohs(hdr->len));
784 raw_dump(level, frm);
785 }
786
787 if (frm->len > btohs(hdr->len)) {
788 frm->len -= btohs(hdr->len);
789 frm->ptr += btohs(hdr->len);
790 } else
791 frm->len = 0;
792 }
793 } else if (cid == 0x2) {
794 /* Connectionless channel */
795
796 if (p_filter(FILT_L2CAP))
797 return;
798
799 psm = btohs(bt_get_unaligned((uint16_t *) frm->ptr));
800 frm->ptr += 2;
801 frm->len -= 2;
802
803 p_indent(level, frm);
804 printf("L2CAP(c): len %d psm %d\n", dlen, psm);
805 raw_dump(level, frm);
806 } else {
807 /* Connection oriented channel */
808
809 uint8_t mode = get_mode(!frm->in, cid);
810 uint16_t psm = get_psm(!frm->in, cid);
811 uint16_t ctrl = 0, fcs = 0;
812 uint32_t proto;
813
814 frm->cid = cid;
815 frm->num = get_num(!frm->in, cid);
816
817 if (mode > 0) {
818 ctrl = btohs(bt_get_unaligned((uint16_t *) frm->ptr));
819 frm->ptr += 2;
820 frm->len -= 4;
821 fcs = btohs(bt_get_unaligned((uint16_t *) (frm->ptr + frm->len)));
822 }
823
824 if (!p_filter(FILT_L2CAP)) {
825 p_indent(level, frm);
826 printf("L2CAP(d): cid 0x%4.4x len %d", cid, dlen);
827 if (mode > 0)
828 printf(" ctrl 0x%4.4x fcs 0x%4.4x", ctrl, fcs);
829 printf(" [psm %d]\n", psm);
830 level++;
831 if (mode > 0) {
832 p_indent(level, frm);
833 printf("%s:", ctrl & 0x01 ? "S-frame" : "I-frame");
834 if (ctrl & 0x01) {
835 printf(" %s", supervisory2str((ctrl & 0x0c) >> 2));
836 } else {
837 uint8_t sar = (ctrl & 0xc000) >> 14;
838 printf(" %s", sar2str(sar));
839 if (sar == 1) {
840 uint16_t len;
841 len = btohs(bt_get_unaligned((uint16_t *) frm->ptr));
842 frm->ptr += 2;
843 frm->len -= 2;
844 printf(" (len %d)", len);
845 }
846 printf(" TxSeq %d", (ctrl & 0x7e) >> 1);
847 }
848 printf(" ReqSeq %d", (ctrl & 0x3f00) >> 8);
849 if (ctrl & 0x80)
850 printf(" F-bit");
851 if (ctrl & 0x10)
852 printf(" P-bit");
853 printf("\n");
854 }
855 }
856
857 switch (psm) {
858 case 0x01:
859 if (!p_filter(FILT_SDP))
860 sdp_dump(level + 1, frm);
861 else
862 raw_dump(level + 1, frm);
863 break;
864
865 case 0x03:
866 if (!p_filter(FILT_RFCOMM))
867 rfcomm_dump(level, frm);
868 else
869 raw_dump(level + 1, frm);
870 break;
871
872 case 0x0f:
873 if (!p_filter(FILT_BNEP))
874 bnep_dump(level, frm);
875 else
876 raw_dump(level + 1, frm);
877 break;
878
879 case 0x11:
880 case 0x13:
881 if (!p_filter(FILT_HIDP))
882 hidp_dump(level, frm);
883 else
884 raw_dump(level + 1, frm);
885 break;
886
887 case 0x17:
888 if (!p_filter(FILT_AVCTP))
889 avctp_dump(level, frm);
890 else
891 raw_dump(level + 1, frm);
892 break;
893
894 case 0x19:
895 if (!p_filter(FILT_AVDTP))
896 avdtp_dump(level, frm);
897 else
898 raw_dump(level + 1, frm);
899 break;
900
901 case 0x1f:
902 if (!p_filter(FILT_ATT))
903 att_dump(level, frm);
904 else
905 raw_dump(level + 1, frm);
906 break;
907
908 default:
909 proto = get_proto(frm->handle, psm, 0);
910
911 switch (proto) {
912 case SDP_UUID_CMTP:
913 if (!p_filter(FILT_CMTP))
914 cmtp_dump(level, frm);
915 else
916 raw_dump(level + 1, frm);
917 break;
918
919 case SDP_UUID_HARDCOPY_CONTROL_CHANNEL:
920 if (!p_filter(FILT_HCRP))
921 hcrp_dump(level, frm);
922 else
923 raw_dump(level + 1, frm);
924 break;
925
926 default:
927 if (p_filter(FILT_L2CAP))
928 break;
929
930 raw_dump(level, frm);
931 break;
932 }
933 break;
934 }
935 }
936 }
937
l2cap_dump(int level,struct frame * frm)938 void l2cap_dump(int level, struct frame *frm)
939 {
940 struct frame *fr;
941 l2cap_hdr *hdr;
942 uint16_t dlen;
943
944 if ((frm->flags & ACL_START) || frm->flags == ACL_START_NO_FLUSH) {
945 hdr = frm->ptr;
946 dlen = btohs(hdr->len);
947
948 if ((int) frm->len == (dlen + L2CAP_HDR_SIZE)) {
949 /* Complete frame */
950 l2cap_parse(level, frm);
951 return;
952 }
953
954 if (!(fr = get_frame(frm->handle))) {
955 fprintf(stderr, "Not enough connection handles\n");
956 raw_dump(level, frm);
957 return;
958 }
959
960 if (fr->data)
961 free(fr->data);
962
963 if (!(fr->data = malloc(dlen + L2CAP_HDR_SIZE))) {
964 perror("Can't allocate L2CAP reassembly buffer");
965 return;
966 }
967 memcpy(fr->data, frm->ptr, frm->len);
968 fr->data_len = dlen + L2CAP_HDR_SIZE;
969 fr->len = frm->len;
970 fr->ptr = fr->data;
971 fr->dev_id = frm->dev_id;
972 fr->in = frm->in;
973 fr->ts = frm->ts;
974 fr->handle = frm->handle;
975 fr->cid = frm->cid;
976 fr->num = frm->num;
977 fr->dlci = frm->dlci;
978 fr->channel = frm->channel;
979 fr->pppdump_fd = frm->pppdump_fd;
980 fr->audio_fd = frm->audio_fd;
981 } else {
982 if (!(fr = get_frame(frm->handle))) {
983 fprintf(stderr, "Not enough connection handles\n");
984 raw_dump(level, frm);
985 return;
986 }
987
988 if (!fr->data) {
989 /* Unexpected fragment */
990 raw_dump(level, frm);
991 return;
992 }
993
994 if (frm->len > (fr->data_len - fr->len)) {
995 /* Bad fragment */
996 raw_dump(level, frm);
997 free(fr->data); fr->data = NULL;
998 return;
999 }
1000
1001 memcpy(fr->data + fr->len, frm->ptr, frm->len);
1002 fr->len += frm->len;
1003
1004 if (fr->len == fr->data_len) {
1005 /* Complete frame */
1006 l2cap_parse(level, fr);
1007
1008 free(fr->data); fr->data = NULL;
1009 return;
1010 }
1011 }
1012 }
1013
l2cap_clear(uint16_t handle)1014 void l2cap_clear(uint16_t handle)
1015 {
1016 del_handle(handle);
1017 }
1018