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-2007 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 default: 310 return "Reserved"; 311 } 312 } 313 314 static char *sar2str(uint8_t sar) 315 { 316 switch (sar) { 317 case 0x00: 318 return "Unsegmented"; 319 case 0x01: 320 return "Start"; 321 case 0x02: 322 return "End"; 323 case 0x03: 324 return "Continuation"; 325 default: 326 return "Bad SAR"; 327 328 } 329 } 330 331 static char *supervisory2str(uint8_t supervisory) 332 { 333 switch (supervisory) { 334 case 0x00: 335 return "Receiver Ready (RR)"; 336 case 0x01: 337 return "Reject (REJ)"; 338 case 0x02: 339 case 0x03: 340 return "Reserved Supervisory"; 341 default: 342 return "Bad Supervisory"; 343 } 344 } 345 346 static inline void command_rej(int level, struct frame *frm) 347 { 348 l2cap_cmd_rej *h = frm->ptr; 349 uint16_t reason = btohs(h->reason); 350 uint32_t cid; 351 352 printf("Command rej: reason %d", reason); 353 354 switch (reason) { 355 case 0x0001: 356 printf(" mtu %d\n", get_val(frm->ptr + L2CAP_CMD_REJ_SIZE, 2)); 357 break; 358 case 0x0002: 359 cid = get_val(frm->ptr + L2CAP_CMD_REJ_SIZE, 4); 360 printf(" dcid 0x%4.4x scid 0x%4.4x\n", cid & 0xffff, cid >> 16); 361 break; 362 default: 363 printf("\n"); 364 break; 365 } 366 367 p_indent(level + 1, frm); 368 printf("%s\n", reason2str(reason)); 369 } 370 371 static inline void conn_req(int level, struct frame *frm) 372 { 373 l2cap_conn_req *h = frm->ptr; 374 uint16_t psm = btohs(h->psm); 375 uint16_t scid = btohs(h->scid); 376 377 add_cid(frm->in, frm->handle, scid, psm); 378 379 if (p_filter(FILT_L2CAP)) 380 return; 381 382 printf("Connect req: psm %d scid 0x%4.4x\n", psm, scid); 383 } 384 385 static inline void conn_rsp(int level, struct frame *frm) 386 { 387 l2cap_conn_rsp *h = frm->ptr; 388 uint16_t scid = btohs(h->scid); 389 uint16_t dcid = btohs(h->dcid); 390 uint16_t result = btohs(h->result); 391 uint16_t status = btohs(h->status); 392 uint16_t psm; 393 394 switch (h->result) { 395 case L2CAP_CR_SUCCESS: 396 if ((psm = get_psm(!frm->in, scid))) 397 add_cid(frm->in, frm->handle, dcid, psm); 398 break; 399 400 case L2CAP_CR_PEND: 401 break; 402 403 default: 404 del_cid(frm->in, dcid, scid); 405 break; 406 } 407 408 if (p_filter(FILT_L2CAP)) 409 return; 410 411 printf("Connect rsp: dcid 0x%4.4x scid 0x%4.4x result %d status %d\n", 412 dcid, scid, result, status); 413 414 p_indent(level + 1, frm); 415 printf("%s", connresult2str(result)); 416 417 if (result == 0x0001) 418 printf(" - %s\n", status2str(status)); 419 else 420 printf("\n"); 421 } 422 423 static void conf_rfc(void *ptr, int len, int in, uint16_t cid) 424 { 425 uint8_t mode; 426 427 mode = *((uint8_t *) ptr); 428 set_mode(in, cid, mode); 429 430 printf("RFC 0x%02x (%s", mode, mode2str(mode)); 431 if (mode == 0x01 || mode == 0x02) { 432 uint8_t txwin, maxtrans; 433 uint16_t rto, mto, mps; 434 txwin = *((uint8_t *) (ptr + 1)); 435 maxtrans = *((uint8_t *) (ptr + 2)); 436 rto = btohs(bt_get_unaligned((uint16_t *) (ptr + 3))); 437 mto = btohs(bt_get_unaligned((uint16_t *) (ptr + 5))); 438 mps = btohs(bt_get_unaligned((uint16_t *) (ptr + 7))); 439 printf(", TxWin %d, MaxTx %d, RTo %d, MTo %d, MPS %d", 440 txwin, maxtrans, rto, mto, mps); 441 } 442 printf(")"); 443 } 444 445 static void conf_opt(int level, void *ptr, int len, int in, uint16_t cid) 446 { 447 p_indent(level, 0); 448 while (len > 0) { 449 l2cap_conf_opt *h = ptr; 450 451 ptr += L2CAP_CONF_OPT_SIZE + h->len; 452 len -= L2CAP_CONF_OPT_SIZE + h->len; 453 454 if (h->type & 0x80) 455 printf("["); 456 457 switch (h->type & 0x7f) { 458 case L2CAP_CONF_MTU: 459 set_mode(in, cid, 0x00); 460 printf("MTU"); 461 if (h->len > 0) 462 printf(" %d", get_val(h->val, h->len)); 463 break; 464 465 case L2CAP_CONF_FLUSH_TO: 466 printf("FlushTO"); 467 if (h->len > 0) 468 printf(" %d", get_val(h->val, h->len)); 469 break; 470 471 case L2CAP_CONF_QOS: 472 printf("QoS"); 473 if (h->len > 0) 474 printf(" 0x%02x (%s)", *(h->val + 1), type2str(*(h->val + 1))); 475 break; 476 477 case L2CAP_CONF_RFC: 478 conf_rfc(h->val, h->len, in, cid); 479 break; 480 481 default: 482 printf("Unknown (type %2.2x, len %d)", h->type & 0x7f, h->len); 483 break; 484 } 485 486 if (h->type & 0x80) 487 printf("] "); 488 else 489 printf(" "); 490 } 491 printf("\n"); 492 } 493 494 static void conf_list(int level, uint8_t *list, int len) 495 { 496 int i; 497 498 p_indent(level, 0); 499 for (i = 0; i < len; i++) { 500 switch (list[i] & 0x7f) { 501 case L2CAP_CONF_MTU: 502 printf("MTU "); 503 break; 504 case L2CAP_CONF_FLUSH_TO: 505 printf("FlushTo "); 506 break; 507 case L2CAP_CONF_QOS: 508 printf("QoS "); 509 break; 510 case L2CAP_CONF_RFC: 511 printf("RFC "); 512 break; 513 default: 514 printf("%2.2x ", list[i] & 0x7f); 515 break; 516 } 517 } 518 printf("\n"); 519 } 520 521 static inline void conf_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm) 522 { 523 l2cap_conf_req *h = frm->ptr; 524 uint16_t dcid = btohs(h->dcid); 525 int clen = btohs(cmd->len) - L2CAP_CONF_REQ_SIZE; 526 527 if (p_filter(FILT_L2CAP)) 528 return; 529 530 printf("Config req: dcid 0x%4.4x flags 0x%2.2x clen %d\n", 531 dcid, btohs(h->flags), clen); 532 533 if (clen > 0) 534 conf_opt(level + 1, h->data, clen, frm->in, dcid); 535 } 536 537 static inline void conf_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm) 538 { 539 l2cap_conf_rsp *h = frm->ptr; 540 uint16_t scid = btohs(h->scid); 541 uint16_t result = btohs(h->result); 542 int clen = btohs(cmd->len) - L2CAP_CONF_RSP_SIZE; 543 544 if (p_filter(FILT_L2CAP)) 545 return; 546 547 printf("Config rsp: scid 0x%4.4x flags 0x%2.2x result %d clen %d\n", 548 scid, btohs(h->flags), result, clen); 549 550 if (clen > 0) { 551 if (result) { 552 p_indent(level + 1, frm); 553 printf("%s\n", confresult2str(result)); 554 } 555 if (result == 0x0003) 556 conf_list(level + 1, h->data, clen); 557 else 558 conf_opt(level + 1, h->data, clen, frm->in, scid); 559 } else { 560 p_indent(level + 1, frm); 561 printf("%s\n", confresult2str(result)); 562 } 563 } 564 565 static inline void disconn_req(int level, struct frame *frm) 566 { 567 l2cap_disconn_req *h = frm->ptr; 568 569 if (p_filter(FILT_L2CAP)) 570 return; 571 572 printf("Disconn req: dcid 0x%4.4x scid 0x%4.4x\n", 573 btohs(h->dcid), btohs(h->scid)); 574 } 575 576 static inline void disconn_rsp(int level, struct frame *frm) 577 { 578 l2cap_disconn_rsp *h = frm->ptr; 579 uint16_t dcid = btohs(h->dcid); 580 uint16_t scid = btohs(h->scid); 581 582 del_cid(frm->in, dcid, scid); 583 584 if (p_filter(FILT_L2CAP)) 585 return; 586 587 printf("Disconn rsp: dcid 0x%4.4x scid 0x%4.4x\n", 588 btohs(h->dcid), btohs(h->scid)); 589 } 590 591 static inline void echo_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm) 592 { 593 if (p_filter(FILT_L2CAP)) 594 return; 595 596 printf("Echo req: dlen %d\n", btohs(cmd->len)); 597 raw_dump(level, frm); 598 } 599 600 static inline void echo_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm) 601 { 602 if (p_filter(FILT_L2CAP)) 603 return; 604 605 printf("Echo rsp: dlen %d\n", btohs(cmd->len)); 606 raw_dump(level, frm); 607 } 608 609 static void info_opt(int level, int type, void *ptr, int len) 610 { 611 uint32_t mask; 612 613 p_indent(level, 0); 614 615 switch (type) { 616 case 0x0001: 617 printf("Connectionless MTU %d\n", get_val(ptr, len)); 618 break; 619 case 0x0002: 620 mask = get_val(ptr, len); 621 printf("Extended feature mask 0x%4.4x\n", mask); 622 if (parser.flags & DUMP_VERBOSE) { 623 if (mask & 0x01) { 624 p_indent(level + 1, 0); 625 printf("Flow control mode\n"); 626 } 627 if (mask & 0x02) { 628 p_indent(level + 1, 0); 629 printf("Retransmission mode\n"); 630 } 631 if (mask & 0x04) { 632 p_indent(level + 1, 0); 633 printf("Bi-directional QoS\n"); 634 } 635 } 636 break; 637 default: 638 printf("Unknown (len %d)\n", len); 639 break; 640 } 641 } 642 643 static inline void info_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm) 644 { 645 l2cap_info_req *h = frm->ptr; 646 647 if (p_filter(FILT_L2CAP)) 648 return; 649 650 printf("Info req: type %d\n", btohs(h->type)); 651 } 652 653 static inline void info_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm) 654 { 655 l2cap_info_rsp *h = frm->ptr; 656 uint16_t type = btohs(h->type); 657 uint16_t result = btohs(h->result); 658 int ilen = btohs(cmd->len) - L2CAP_INFO_RSP_SIZE; 659 660 if (p_filter(FILT_L2CAP)) 661 return; 662 663 printf("Info rsp: type %d result %d\n", type, result); 664 665 if (ilen > 0) { 666 info_opt(level + 1, type, h->data, ilen); 667 } else { 668 p_indent(level + 1, frm); 669 printf("%s\n", inforesult2str(result)); 670 } 671 } 672 673 static void l2cap_parse(int level, struct frame *frm) 674 { 675 l2cap_hdr *hdr = (void *)frm->ptr; 676 uint16_t dlen = btohs(hdr->len); 677 uint16_t cid = btohs(hdr->cid); 678 uint16_t psm; 679 680 frm->ptr += L2CAP_HDR_SIZE; 681 frm->len -= L2CAP_HDR_SIZE; 682 683 if (cid == 0x1) { 684 /* Signaling channel */ 685 686 while (frm->len >= L2CAP_CMD_HDR_SIZE) { 687 l2cap_cmd_hdr *hdr = frm->ptr; 688 689 frm->ptr += L2CAP_CMD_HDR_SIZE; 690 frm->len -= L2CAP_CMD_HDR_SIZE; 691 692 if (!p_filter(FILT_L2CAP)) { 693 p_indent(level, frm); 694 printf("L2CAP(s): "); 695 } 696 697 switch (hdr->code) { 698 case L2CAP_COMMAND_REJ: 699 command_rej(level, frm); 700 break; 701 702 case L2CAP_CONN_REQ: 703 conn_req(level, frm); 704 break; 705 706 case L2CAP_CONN_RSP: 707 conn_rsp(level, frm); 708 break; 709 710 case L2CAP_CONF_REQ: 711 conf_req(level, hdr, frm); 712 break; 713 714 case L2CAP_CONF_RSP: 715 conf_rsp(level, hdr, frm); 716 break; 717 718 case L2CAP_DISCONN_REQ: 719 disconn_req(level, frm); 720 break; 721 722 case L2CAP_DISCONN_RSP: 723 disconn_rsp(level, frm); 724 break; 725 726 case L2CAP_ECHO_REQ: 727 echo_req(level, hdr, frm); 728 break; 729 730 case L2CAP_ECHO_RSP: 731 echo_rsp(level, hdr, frm); 732 break; 733 734 case L2CAP_INFO_REQ: 735 info_req(level, hdr, frm); 736 break; 737 738 case L2CAP_INFO_RSP: 739 info_rsp(level, hdr, frm); 740 break; 741 742 default: 743 if (p_filter(FILT_L2CAP)) 744 break; 745 printf("code 0x%2.2x ident %d len %d\n", 746 hdr->code, hdr->ident, btohs(hdr->len)); 747 raw_dump(level, frm); 748 } 749 750 if (frm->len > btohs(hdr->len)) { 751 frm->len -= btohs(hdr->len); 752 frm->ptr += btohs(hdr->len); 753 } else 754 frm->len = 0; 755 } 756 } else if (cid == 0x2) { 757 /* Connectionless channel */ 758 759 if (p_filter(FILT_L2CAP)) 760 return; 761 762 psm = btohs(bt_get_unaligned((uint16_t *) frm->ptr)); 763 frm->ptr += 2; 764 frm->len -= 2; 765 766 p_indent(level, frm); 767 printf("L2CAP(c): len %d psm %d\n", dlen, psm); 768 raw_dump(level, frm); 769 } else { 770 /* Connection oriented channel */ 771 772 uint8_t mode = get_mode(!frm->in, cid); 773 uint16_t psm = get_psm(!frm->in, cid); 774 uint16_t ctrl = 0, fcs = 0; 775 uint32_t proto; 776 777 frm->cid = cid; 778 frm->num = get_num(!frm->in, cid); 779 780 if (mode > 0) { 781 ctrl = btohs(bt_get_unaligned((uint16_t *) frm->ptr)); 782 frm->ptr += 2; 783 frm->len -= 4; 784 fcs = btohs(bt_get_unaligned((uint16_t *) (frm->ptr + frm->len))); 785 } 786 787 if (!p_filter(FILT_L2CAP)) { 788 p_indent(level, frm); 789 printf("L2CAP(d): cid 0x%4.4x len %d", cid, dlen); 790 if (mode > 0) 791 printf(" ctrl 0x%4.4x fcs 0x%4.4x", ctrl, fcs); 792 printf(" [psm %d]\n", psm); 793 level++; 794 if (mode > 0) { 795 p_indent(level, frm); 796 printf("%s:", ctrl & 0x01 ? "S-frame" : "I-frame"); 797 if (ctrl & 0x01) { 798 printf(" %s", supervisory2str((ctrl & 0x0c) >> 2)); 799 } else { 800 uint8_t sar = (ctrl & 0xc000) >> 14; 801 printf(" %s", sar2str(sar)); 802 if (sar == 1) { 803 uint16_t len; 804 len = btohs(bt_get_unaligned((uint16_t *) frm->ptr)); 805 frm->ptr += 2; 806 frm->len -= 2; 807 printf(" (len %d)", len); 808 } 809 printf(" TxSeq %d", (ctrl & 0x7e) >> 1); 810 } 811 printf(" ReqSeq %d", (ctrl & 0x3f00) >> 8); 812 if (ctrl & 0x80) 813 printf(" Retransmission Disable"); 814 printf("\n"); 815 } 816 } 817 818 switch (psm) { 819 case 0x01: 820 if (!p_filter(FILT_SDP)) 821 sdp_dump(level + 1, frm); 822 else 823 raw_dump(level + 1, frm); 824 break; 825 826 case 0x03: 827 if (!p_filter(FILT_RFCOMM)) 828 rfcomm_dump(level, frm); 829 else 830 raw_dump(level + 1, frm); 831 break; 832 833 case 0x0f: 834 if (!p_filter(FILT_BNEP)) 835 bnep_dump(level, frm); 836 else 837 raw_dump(level + 1, frm); 838 break; 839 840 case 0x11: 841 case 0x13: 842 if (!p_filter(FILT_HIDP)) 843 hidp_dump(level, frm); 844 else 845 raw_dump(level + 1, frm); 846 break; 847 848 case 0x17: 849 if (!p_filter(FILT_AVCTP)) 850 avctp_dump(level, frm); 851 else 852 raw_dump(level + 1, frm); 853 break; 854 855 case 0x19: 856 if (!p_filter(FILT_AVDTP)) 857 avdtp_dump(level, frm); 858 else 859 raw_dump(level + 1, frm); 860 break; 861 862 default: 863 proto = get_proto(frm->handle, psm, 0); 864 865 switch (proto) { 866 case SDP_UUID_CMTP: 867 if (!p_filter(FILT_CMTP)) 868 cmtp_dump(level, frm); 869 else 870 raw_dump(level + 1, frm); 871 break; 872 873 case SDP_UUID_HARDCOPY_CONTROL_CHANNEL: 874 if (!p_filter(FILT_HCRP)) 875 hcrp_dump(level, frm); 876 else 877 raw_dump(level + 1, frm); 878 break; 879 880 default: 881 if (p_filter(FILT_L2CAP)) 882 break; 883 884 raw_dump(level, frm); 885 break; 886 } 887 break; 888 } 889 } 890 } 891 892 void l2cap_dump(int level, struct frame *frm) 893 { 894 struct frame *fr; 895 l2cap_hdr *hdr; 896 uint16_t dlen; 897 898 if (frm->flags & ACL_START) { 899 hdr = frm->ptr; 900 dlen = btohs(hdr->len); 901 902 if (frm->len == (dlen + L2CAP_HDR_SIZE)) { 903 /* Complete frame */ 904 l2cap_parse(level, frm); 905 return; 906 } 907 908 if (!(fr = get_frame(frm->handle))) { 909 fprintf(stderr, "Not enough connection handles\n"); 910 raw_dump(level, frm); 911 return; 912 } 913 914 if (fr->data) 915 free(fr->data); 916 917 if (!(fr->data = malloc(dlen + L2CAP_HDR_SIZE))) { 918 perror("Can't allocate L2CAP reassembly buffer"); 919 return; 920 } 921 memcpy(fr->data, frm->ptr, frm->len); 922 fr->data_len = dlen + L2CAP_HDR_SIZE; 923 fr->len = frm->len; 924 fr->ptr = fr->data; 925 fr->dev_id = frm->dev_id; 926 fr->in = frm->in; 927 fr->ts = frm->ts; 928 fr->handle = frm->handle; 929 fr->cid = frm->cid; 930 fr->num = frm->num; 931 fr->dlci = frm->dlci; 932 fr->channel = frm->channel; 933 fr->pppdump_fd = frm->pppdump_fd; 934 fr->audio_fd = frm->audio_fd; 935 } else { 936 if (!(fr = get_frame(frm->handle))) { 937 fprintf(stderr, "Not enough connection handles\n"); 938 raw_dump(level, frm); 939 return; 940 } 941 942 if (!fr->data) { 943 /* Unexpected fragment */ 944 raw_dump(level, frm); 945 return; 946 } 947 948 if (frm->len > (fr->data_len - fr->len)) { 949 /* Bad fragment */ 950 raw_dump(level, frm); 951 free(fr->data); fr->data = NULL; 952 return; 953 } 954 955 memcpy(fr->data + fr->len, frm->ptr, frm->len); 956 fr->len += frm->len; 957 958 if (fr->len == fr->data_len) { 959 /* Complete frame */ 960 l2cap_parse(level, fr); 961 962 free(fr->data); fr->data = NULL; 963 return; 964 } 965 } 966 } 967 968 void l2cap_clear(uint16_t handle) 969 { 970 del_handle(handle); 971 } 972
