1 /* 2 * 3 * BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2000-2002 Maxim Krasnyansky <maxk (at) qualcomm.com> 6 * Copyright (C) 2003-2011 Marcel Holtmann <marcel (at) 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 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 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 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 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 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 } 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 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 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 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 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 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 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 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 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 } 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 1014 void l2cap_clear(uint16_t handle) 1015 { 1016 del_handle(handle); 1017 } 1018
