tools/parser/l2cap.c

   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 #include <inttypes.h>
  35
  36 #include "parser.h"
  37 #include "sdp.h"
  38 #include "l2cap.h"
  39 #include "lib/hci.h"
  40 #include "lib/a2mp.h"
  41 #include "lib/amp.h"
  42
  43 typedef struct {
  44         uint16_t handle;
  45         struct frame frm;
  46 } handle_info;
  47 #define HANDLE_TABLE_SIZE 10
  48
  49 static handle_info handle_table[HANDLE_TABLE_SIZE];
  50
  51 typedef struct {
  52         uint16_t handle;
  53         uint16_t cid;
  54         uint16_t psm;
  55         uint16_t num;
  56         uint8_t mode;
  57         uint8_t ext_ctrl;
  58 } cid_info;
  59 #define CID_TABLE_SIZE 20
  60
  61 static cid_info cid_table[2][CID_TABLE_SIZE];
  62
  63 #define SCID cid_table[0]
  64 #define DCID cid_table[1]
  65
  66 /* Can we move this to l2cap.h? */
  67 struct features {
  68         char    *name;
  69         int     flag;
  70 };
  71
  72 static struct features l2cap_features[] = {
  73         { "Flow control mode",                  L2CAP_FEAT_FLOWCTL      },
  74         { "Retransmission mode",                L2CAP_FEAT_RETRANS      },
  75         { "Bi-directional QoS",                 L2CAP_FEAT_BIDIR_QOS    },
  76         { "Enhanced Retransmission mode",       L2CAP_FEAT_ERTM         },
  77         { "Streaming mode",                     L2CAP_FEAT_STREAMING    },
  78         { "FCS Option",                         L2CAP_FEAT_FCS          },
  79         { "Extended Flow Specification",        L2CAP_FEAT_EXT_FLOW     },
  80         { "Fixed Channels",                     L2CAP_FEAT_FIXED_CHAN   },
  81         { "Extended Window Size",               L2CAP_FEAT_EXT_WINDOW   },
  82         { "Unicast Connectless Data Reception", L2CAP_FEAT_UCD          },
  83         { 0 }
  84 };
  85
  86 static struct features l2cap_fix_chan[] = {
  87         { "L2CAP Signalling Channel",           L2CAP_FC_L2CAP          },
  88         { "L2CAP Connless",                     L2CAP_FC_CONNLESS       },
  89         { "AMP Manager Protocol",               L2CAP_FC_A2MP           },
  90         { 0 }
  91 };
  92
  93 static struct frame *add_handle(uint16_t handle)
  94 {
  95         register handle_info *t = handle_table;
  96         register int i;
  97
  98         for (i = 0; i < HANDLE_TABLE_SIZE; i++)
  99                 if (!t[i].handle) {
 100                         t[i].handle = handle;
 101                         return &t[i].frm;
 102                 }
 103         return NULL;
 104 }
 105
 106 static struct frame *get_frame(uint16_t handle)
 107 {
 108         register handle_info *t = handle_table;
 109         register int i;
 110
 111         for (i = 0; i < HANDLE_TABLE_SIZE; i++)
 112                 if (t[i].handle == handle)
 113                         return &t[i].frm;
 114
 115         return add_handle(handle);
 116 }
 117
 118 static void add_cid(int in, uint16_t handle, uint16_t cid, uint16_t psm)
 119 {
 120         register cid_info *table = cid_table[in];
 121         register int i, pos = -1;
 122         uint16_t num = 1;
 123
 124         for (i = 0; i < CID_TABLE_SIZE; i++) {
 125                 if ((pos < 0 && !table[i].cid) || table[i].cid == cid)
 126                         pos = i;
 127                 if (table[i].psm == psm)
 128                         num++;
 129         }
 130
 131         if (pos >= 0) {
 132                 table[pos].handle = handle;
 133                 table[pos].cid    = cid;
 134                 table[pos].psm    = psm;
 135                 table[pos].num    = num;
 136                 table[pos].mode   = 0;
 137         }
 138 }
 139
 140 static void del_cid(int in, uint16_t dcid, uint16_t scid)
 141 {
 142         register int t, i;
 143         uint16_t cid[2];
 144
 145         if (!in) {
 146                 cid[0] = dcid;
 147                 cid[1] = scid;
 148         } else {
 149                 cid[0] = scid;
 150                 cid[1] = dcid;
 151         }
 152
 153         for (t = 0; t < 2; t++) {
 154                 for (i = 0; i < CID_TABLE_SIZE; i++)
 155                         if (cid_table[t][i].cid == cid[t]) {
 156                                 cid_table[t][i].handle = 0;
 157                                 cid_table[t][i].cid    = 0;
 158                                 cid_table[t][i].psm    = 0;
 159                                 cid_table[t][i].num    = 0;
 160                                 cid_table[t][i].mode   = 0;
 161                                 break;
 162                         }
 163         }
 164 }
 165
 166 static void del_handle(uint16_t handle)
 167 {
 168         register int t, i;
 169
 170         for (t = 0; t < 2; t++) {
 171                 for (i = 0; i < CID_TABLE_SIZE; i++)
 172                         if (cid_table[t][i].handle == handle) {
 173                                 cid_table[t][i].handle = 0;
 174                                 cid_table[t][i].cid    = 0;
 175                                 cid_table[t][i].psm    = 0;
 176                                 cid_table[t][i].num    = 0;
 177                                 cid_table[t][i].mode   = 0;
 178                                 break;
 179                         }
 180         }
 181 }
 182 static uint16_t get_psm(int in, uint16_t handle, uint16_t cid)
 183 {
 184         register cid_info *table = cid_table[in];
 185         register int i;
 186
 187         for (i = 0; i < CID_TABLE_SIZE; i++)
 188                 if (table[i].handle == handle && table[i].cid == cid)
 189                         return table[i].psm;
 190         return parser.defpsm;
 191 }
 192
 193 static uint16_t get_num(int in, uint16_t handle, uint16_t cid)
 194 {
 195         register cid_info *table = cid_table[in];
 196         register int i;
 197
 198         for (i = 0; i < CID_TABLE_SIZE; i++)
 199                 if (table[i].handle == handle && table[i].cid == cid)
 200                         return table[i].num;
 201         return 0;
 202 }
 203
 204 static void set_mode(int in, uint16_t handle, uint16_t cid, uint8_t mode)
 205 {
 206         register cid_info *table = cid_table[in];
 207         register int i;
 208
 209         for (i = 0; i < CID_TABLE_SIZE; i++)
 210                 if (table[i].handle == handle && table[i].cid == cid)
 211                         table[i].mode = mode;
 212 }
 213
 214 static uint8_t get_mode(int in, uint16_t handle, uint16_t cid)
 215 {
 216         register cid_info *table = cid_table[in];
 217         register int i;
 218
 219         for (i = 0; i < CID_TABLE_SIZE; i++)
 220                 if (table[i].handle == handle && table[i].cid == cid)
 221                         return table[i].mode;
 222         return 0;
 223 }
 224
 225 static void set_ext_ctrl(int in, uint16_t handle, uint16_t cid,
 226                                                         uint8_t ext_ctrl)
 227 {
 228         register cid_info *table = cid_table[in];
 229         register int i;
 230
 231         for (i = 0; i < CID_TABLE_SIZE; i++)
 232                 if (table[i].handle == handle && table[i].cid == cid)
 233                         table[i].ext_ctrl = ext_ctrl;
 234 }
 235
 236 static uint8_t get_ext_ctrl(int in, uint16_t handle, uint16_t cid)
 237 {
 238         register cid_info *table = cid_table[in];
 239         register int i;
 240
 241         for (i = 0; i < CID_TABLE_SIZE; i++)
 242                 if (table[i].handle == handle && table[i].cid == cid)
 243                         return table[i].ext_ctrl;
 244         return 0;
 245 }
 246
 247 static uint32_t get_val(uint8_t *ptr, uint8_t len)
 248 {
 249         switch (len) {
 250         case 1:
 251                 return *ptr;
 252         case 2:
 253                 return get_le16(ptr);
 254         case 4:
 255                 return get_le32(ptr);
 256         }
 257         return 0;
 258 }
 259
 260 static char *reason2str(uint16_t reason)
 261 {
 262         switch (reason) {
 263         case 0x0000:
 264                 return "Command not understood";
 265         case 0x0001:
 266                 return "Signalling MTU exceeded";
 267         case 0x0002:
 268                 return "Invalid CID in request";
 269         default:
 270                 return "Reserved";
 271         }
 272 }
 273
 274 static char *a2mpreason2str(uint16_t reason)
 275 {
 276         switch (reason) {
 277         case A2MP_COMMAND_NOT_RECOGNIZED:
 278                 return "Command not recognized";
 279         default:
 280                 return "Reserved";
 281         }
 282 }
 283
 284 static char *connresult2str(uint16_t result)
 285 {
 286         switch (result) {
 287         case 0x0000:
 288                 return "Connection successful";
 289         case 0x0001:
 290                 return "Connection pending";
 291         case 0x0002:
 292                 return "Connection refused - PSM not supported";
 293         case 0x0003:
 294                 return "Connection refused - security block";
 295         case 0x0004:
 296                 return "Connection refused - no resources available";
 297         default:
 298                 return "Reserved";
 299         }
 300 }
 301
 302 static char *status2str(uint16_t status)
 303 {
 304         switch (status) {
 305         case 0x0000:
 306                 return "No futher information available";
 307         case 0x0001:
 308                 return "Authentication pending";
 309         case 0x0002:
 310                 return "Authorization pending";
 311         default:
 312                 return "Reserved";
 313         }
 314 }
 315
 316 static char *confresult2str(uint16_t result)
 317 {
 318         switch (result) {
 319         case L2CAP_CONF_SUCCESS:
 320                 return "Success";
 321         case L2CAP_CONF_UNACCEPT:
 322                 return "Failure - unacceptable parameters";
 323         case L2CAP_CONF_REJECT:
 324                 return "Failure - rejected (no reason provided)";
 325         case L2CAP_CONF_UNKNOWN:
 326                 return "Failure - unknown options";
 327         case L2CAP_CONF_PENDING:
 328                 return "Pending";
 329         case L2CAP_CONF_EFS_REJECT:
 330                 return "Failure - flowspec reject";
 331         default:
 332                 return "Reserved";
 333         }
 334 }
 335 static char *inforesult2str(uint16_t result)
 336 {
 337         switch (result) {
 338         case 0x0000:
 339                 return "Success";
 340         case 0x0001:
 341                 return "Not supported";
 342         default:
 343                 return "Reserved";
 344         }
 345 }
 346
 347 static char *type2str(uint8_t type)
 348 {
 349         switch (type) {
 350         case L2CAP_SERVTYPE_NOTRAFFIC:
 351                 return "No traffic";
 352         case L2CAP_SERVTYPE_BESTEFFORT:
 353                 return "Best Effort";
 354         case L2CAP_SERVTYPE_GUARANTEED:
 355                 return "Guaranteed";
 356         default:
 357                 return "Reserved";
 358         }
 359 }
 360
 361 static char *mode2str(uint8_t mode)
 362 {
 363         switch (mode) {
 364         case 0x00:
 365                 return "Basic";
 366         case 0x01:
 367                 return "Retransmission";
 368         case 0x02:
 369                 return "Flow control";
 370         case 0x03:
 371                 return "Enhanced Retransmission";
 372         case 0x04:
 373                 return "Streaming";
 374         default:
 375                 return "Reserved";
 376         }
 377 }
 378
 379 static char *fcs2str(uint8_t fcs)
 380 {
 381         switch (fcs) {
 382         case 0x00:
 383                 return "No FCS";
 384         case 0x01:
 385                 return "CRC16 Check";
 386         default:
 387                 return "Reserved";
 388         }
 389 }
 390
 391 static char *sar2str(uint8_t sar)
 392 {
 393         switch (sar) {
 394         case L2CAP_SAR_UNSEGMENTED:
 395                 return "Unsegmented";
 396         case L2CAP_SAR_START:
 397                 return "Start";
 398         case L2CAP_SAR_END:
 399                 return "End";
 400         case L2CAP_SAR_CONTINUE:
 401                 return "Continuation";
 402         default:
 403                 return "Bad SAR";
 404
 405         }
 406 }
 407
 408 static char *supervisory2str(uint8_t supervisory)
 409 {
 410         switch (supervisory) {
 411         case L2CAP_SUPER_RR:
 412                 return "Receiver Ready (RR)";
 413         case L2CAP_SUPER_REJ:
 414                 return "Reject (REJ)";
 415         case L2CAP_SUPER_RNR:
 416                 return "Receiver Not Ready (RNR)";
 417         case L2CAP_SUPER_SREJ:
 418                 return "Select Reject (SREJ)";
 419         default:
 420                 return "Bad Supervisory";
 421         }
 422 }
 423
 424 static char *ampctrltype2str(uint8_t type)
 425 {
 426         switch (type) {
 427         case HCI_BREDR:
 428                 return "BR-EDR";
 429         case HCI_AMP:
 430                 return "802.11 AMP";
 431         default:
 432                 return "Reserved";
 433         }
 434 }
 435
 436 static char *ampctrlstatus2str(uint8_t status)
 437 {
 438         switch (status) {
 439         case AMP_CTRL_POWERED_DOWN:
 440                 return "Powered down";
 441         case AMP_CTRL_BLUETOOTH_ONLY:
 442                 return "Bluetooth only";
 443         case AMP_CTRL_NO_CAPACITY:
 444                 return "No capacity";
 445         case AMP_CTRL_LOW_CAPACITY:
 446                 return "Low capacity";
 447         case AMP_CTRL_MEDIUM_CAPACITY:
 448                 return "Medium capacity";
 449         case AMP_CTRL_HIGH_CAPACITY:
 450                 return "High capacity";
 451         case AMP_CTRL_FULL_CAPACITY:
 452                 return "Full capacity";
 453         default:
 454                 return "Reserved";
 455
 456         }
 457 }
 458
 459 static char *a2mpstatus2str(uint8_t status)
 460 {
 461         switch (status) {
 462         case A2MP_STATUS_SUCCESS:
 463                 return "Success";
 464         case A2MP_STATUS_INVALID_CTRL_ID:
 465                 return "Invalid Controller ID";
 466         default:
 467                 return "Reserved";
 468         }
 469 }
 470
 471 static char *a2mpcplstatus2str(uint8_t status)
 472 {
 473         switch (status) {
 474         case A2MP_STATUS_SUCCESS:
 475                 return "Success";
 476         case A2MP_STATUS_INVALID_CTRL_ID:
 477                 return "Invalid Controller ID";
 478         case A2MP_STATUS_UNABLE_START_LINK_CREATION:
 479                 return "Failed - Unable to start link creation";
 480         case A2MP_STATUS_COLLISION_OCCURED:
 481                 return "Failed - Collision occured";
 482         case A2MP_STATUS_DISCONN_REQ_RECVD:
 483                 return "Failed - Disconnect physical link received";
 484         case A2MP_STATUS_PHYS_LINK_EXISTS:
 485                 return "Failed - Physical link already exists";
 486         case A2MP_STATUS_SECURITY_VIOLATION:
 487                 return "Failed - Security violation";
 488         default:
 489                 return "Reserved";
 490         }
 491 }
 492
 493 static char *a2mpdplstatus2str(uint8_t status)
 494 {
 495         switch (status) {
 496         case A2MP_STATUS_SUCCESS:
 497                 return "Success";
 498         case A2MP_STATUS_INVALID_CTRL_ID:
 499                 return "Invalid Controller ID";
 500         case A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS:
 501                 return "Failed - No Physical Link exists";
 502         default:
 503                 return "Reserved";
 504         }
 505 }
 506
 507 static inline void command_rej(int level, struct frame *frm)
 508 {
 509         l2cap_cmd_rej *h = frm->ptr;
 510         uint16_t reason = btohs(h->reason);
 511         uint32_t cid;
 512
 513         printf("Command rej: reason %d", reason);
 514
 515         switch (reason) {
 516         case 0x0001:
 517                 printf(" mtu %d\n", get_val(frm->ptr + L2CAP_CMD_REJ_SIZE, 2));
 518                 break;
 519         case 0x0002:
 520                 cid = get_val(frm->ptr + L2CAP_CMD_REJ_SIZE, 4);
 521                 printf(" dcid 0x%4.4x scid 0x%4.4x\n", cid & 0xffff, cid >> 16);
 522                 break;
 523         default:
 524                 printf("\n");
 525                 break;
 526         }
 527
 528         p_indent(level + 1, frm);
 529         printf("%s\n", reason2str(reason));
 530 }
 531
 532 static inline void conn_req(int level, struct frame *frm)
 533 {
 534         l2cap_conn_req *h = frm->ptr;
 535         uint16_t psm = btohs(h->psm);
 536         uint16_t scid = btohs(h->scid);
 537
 538         add_cid(frm->in, frm->handle, scid, psm);
 539
 540         if (p_filter(FILT_L2CAP))
 541                 return;
 542
 543         printf("Connect req: psm %d scid 0x%4.4x\n", psm, scid);
 544 }
 545
 546 static inline void conn_rsp(int level, struct frame *frm)
 547 {
 548         l2cap_conn_rsp *h = frm->ptr;
 549         uint16_t scid = btohs(h->scid);
 550         uint16_t dcid = btohs(h->dcid);
 551         uint16_t result = btohs(h->result);
 552         uint16_t status = btohs(h->status);
 553         uint16_t psm;
 554
 555         switch (h->result) {
 556         case L2CAP_CR_SUCCESS:
 557                 if ((psm = get_psm(!frm->in, frm->handle, scid)))
 558                         add_cid(frm->in, frm->handle, dcid, psm);
 559                 break;
 560
 561         case L2CAP_CR_PEND:
 562                 break;
 563
 564         default:
 565                 del_cid(frm->in, dcid, scid);
 566                 break;
 567         }
 568
 569         if (p_filter(FILT_L2CAP))
 570                 return;
 571
 572         printf("Connect rsp: dcid 0x%4.4x scid 0x%4.4x result %d status %d\n",
 573                 dcid, scid, result, status);
 574
 575         p_indent(level + 1, frm);
 576         printf("%s", connresult2str(result));
 577
 578         if (result == 0x0001)
 579                 printf(" - %s\n", status2str(status));
 580         else
 581                 printf("\n");
 582 }
 583
 584 static void conf_rfc(void *ptr, int len, int in, uint16_t handle,
 585                                                                 uint16_t cid)
 586 {
 587         uint8_t mode;
 588
 589         mode = *((uint8_t *) ptr);
 590         set_mode(!in, handle, cid, mode);
 591
 592         printf("RFC 0x%02x (%s", mode, mode2str(mode));
 593         if (mode >= 0x01 && mode <= 0x04) {
 594                 uint8_t txwin, maxtrans;
 595                 uint16_t rto, mto, mps;
 596                 txwin = *((uint8_t *) (ptr + 1));
 597                 maxtrans = *((uint8_t *) (ptr + 2));
 598                 rto = get_le16(ptr + 3);
 599                 mto = get_le16(ptr + 5);
 600                 mps = get_le16(ptr + 7);
 601                 printf(", TxWin %d, MaxTx %d, RTo %d, MTo %d, MPS %d",
 602                                         txwin, maxtrans, rto, mto, mps);
 603         }
 604         printf(")");
 605 }
 606
 607 static void conf_efs(void *ptr)
 608 {
 609         uint8_t id, ser_type;
 610         uint16_t max_sdu;
 611         uint32_t sdu_itime, access_lat, flush_to;
 612
 613         id = get_val(ptr, sizeof(id));
 614         ser_type = get_val(ptr + 1, sizeof(ser_type));
 615         max_sdu = get_val(ptr + 2, sizeof(max_sdu));
 616         sdu_itime = get_val(ptr + 4, sizeof(sdu_itime));
 617         access_lat = get_val(ptr + 8, sizeof(access_lat));
 618         flush_to = get_val(ptr + 12, sizeof(flush_to));
 619
 620         printf("EFS (Id 0x%02x, SerType %s, MaxSDU 0x%04x, SDUitime 0x%08x, "
 621                         "AccLat 0x%08x, FlushTO 0x%08x)",
 622                         id, type2str(ser_type), max_sdu, sdu_itime,
 623                         access_lat, flush_to);
 624 }
 625
 626 static void conf_fcs(void *ptr, int len)
 627 {
 628         uint8_t fcs;
 629
 630         fcs = *((uint8_t *) ptr);
 631         printf("FCS Option");
 632         if (len > 0)
 633                 printf(" 0x%2.2x (%s)", fcs, fcs2str(fcs));
 634 }
 635
 636 static void conf_opt(int level, void *ptr, int len, int in, uint16_t handle,
 637                                                                 uint16_t cid)
 638 {
 639         int indent = 0;
 640         p_indent(level, 0);
 641         while (len > 0) {
 642                 l2cap_conf_opt *h = ptr;
 643
 644                 ptr += L2CAP_CONF_OPT_SIZE + h->len;
 645                 len -= L2CAP_CONF_OPT_SIZE + h->len;
 646
 647                 if (h->type & 0x80)
 648                         printf("[");
 649
 650                 if (indent++) {
 651                         printf("\n");
 652                         p_indent(level, 0);
 653                 }
 654
 655                 switch (h->type & 0x7f) {
 656                 case L2CAP_CONF_MTU:
 657                         set_mode(in, handle, cid, 0x00);
 658                         printf("MTU");
 659                         if (h->len > 0)
 660                                 printf(" %d", get_val(h->val, h->len));
 661                         break;
 662
 663                 case L2CAP_CONF_FLUSH_TO:
 664                         printf("FlushTO");
 665                         if (h->len > 0)
 666                                 printf(" %d", get_val(h->val, h->len));
 667                         break;
 668
 669                 case L2CAP_CONF_QOS:
 670                         printf("QoS");
 671                         if (h->len > 0)
 672                                 printf(" 0x%02x (%s)", *(h->val + 1), type2str(*(h->val + 1)));
 673                         break;
 674
 675                 case L2CAP_CONF_RFC:
 676                         conf_rfc(h->val, h->len, in, handle, cid);
 677                         break;
 678
 679                 case L2CAP_CONF_FCS:
 680                         conf_fcs(h->val, h->len);
 681                         break;
 682
 683                 case L2CAP_CONF_EFS:
 684                         conf_efs(h->val);
 685                         break;
 686
 687                 case L2CAP_CONF_EWS:
 688                         printf("EWS");
 689                         if (h->len > 0)
 690                                 printf(" %d", get_val(h->val, h->len));
 691                         set_ext_ctrl(in, handle, cid, 1);
 692                         break;
 693
 694                 default:
 695                         printf("Unknown (type %2.2x, len %d)", h->type & 0x7f, h->len);
 696                         break;
 697                 }
 698
 699                 if (h->type & 0x80)
 700                         printf("] ");
 701                 else
 702                         printf(" ");
 703         }
 704         printf("\n");
 705 }
 706
 707 static void conf_list(int level, uint8_t *list, int len)
 708 {
 709         int i;
 710
 711         p_indent(level, 0);
 712         for (i = 0; i < len; i++) {
 713                 switch (list[i] & 0x7f) {
 714                 case L2CAP_CONF_MTU:
 715                         printf("MTU ");
 716                         break;
 717                 case L2CAP_CONF_FLUSH_TO:
 718                         printf("FlushTo ");
 719                         break;
 720                 case L2CAP_CONF_QOS:
 721                         printf("QoS ");
 722                         break;
 723                 case L2CAP_CONF_RFC:
 724                         printf("RFC ");
 725                         break;
 726                 case L2CAP_CONF_FCS:
 727                         printf("FCS ");
 728                         break;
 729                 case L2CAP_CONF_EFS:
 730                         printf("EFS ");
 731                         break;
 732                 case L2CAP_CONF_EWS:
 733                         printf("EWS ");
 734                         break;
 735                 default:
 736                         printf("%2.2x ", list[i] & 0x7f);
 737                         break;
 738                 }
 739         }
 740         printf("\n");
 741 }
 742
 743 static inline void conf_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
 744 {
 745         l2cap_conf_req *h = frm->ptr;
 746         uint16_t dcid = btohs(h->dcid);
 747         int clen = btohs(cmd->len) - L2CAP_CONF_REQ_SIZE;
 748
 749         if (p_filter(FILT_L2CAP))
 750                 return;
 751
 752         printf("Config req: dcid 0x%4.4x flags 0x%2.2x clen %d\n",
 753                         dcid, btohs(h->flags), clen);
 754
 755         if (clen > 0)
 756                 conf_opt(level + 1, h->data, clen, frm->in, frm->handle,
 757                                                                         dcid);
 758 }
 759
 760 static inline void conf_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
 761 {
 762         l2cap_conf_rsp *h = frm->ptr;
 763         uint16_t scid = btohs(h->scid);
 764         uint16_t result = btohs(h->result);
 765         int clen = btohs(cmd->len) - L2CAP_CONF_RSP_SIZE;
 766
 767         if (p_filter(FILT_L2CAP))
 768                 return;
 769
 770         printf("Config rsp: scid 0x%4.4x flags 0x%2.2x result %d clen %d\n",
 771                         scid, btohs(h->flags), result, clen);
 772
 773         if (clen > 0) {
 774                 if (result) {
 775                         p_indent(level + 1, frm);
 776                         printf("%s\n", confresult2str(result));
 777                 }
 778                 if (result == 0x0003)
 779                         conf_list(level + 1, h->data, clen);
 780                 else
 781                         conf_opt(level + 1, h->data, clen, frm->in,
 782                                                         frm->handle, scid);
 783         } else {
 784                 p_indent(level + 1, frm);
 785                 printf("%s\n", confresult2str(result));
 786         }
 787 }
 788
 789 static inline void disconn_req(int level, struct frame *frm)
 790 {
 791         l2cap_disconn_req *h = frm->ptr;
 792
 793         if (p_filter(FILT_L2CAP))
 794                 return;
 795
 796         printf("Disconn req: dcid 0x%4.4x scid 0x%4.4x\n",
 797                         btohs(h->dcid), btohs(h->scid));
 798 }
 799
 800 static inline void disconn_rsp(int level, struct frame *frm)
 801 {
 802         l2cap_disconn_rsp *h = frm->ptr;
 803         uint16_t dcid = btohs(h->dcid);
 804         uint16_t scid = btohs(h->scid);
 805
 806         del_cid(frm->in, dcid, scid);
 807
 808         if (p_filter(FILT_L2CAP))
 809                 return;
 810
 811         printf("Disconn rsp: dcid 0x%4.4x scid 0x%4.4x\n",
 812                         btohs(h->dcid), btohs(h->scid));
 813 }
 814
 815 static inline void echo_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
 816 {
 817         if (p_filter(FILT_L2CAP))
 818                 return;
 819
 820         printf("Echo req: dlen %d\n", btohs(cmd->len));
 821         raw_dump(level, frm);
 822 }
 823
 824 static inline void echo_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
 825 {
 826         if (p_filter(FILT_L2CAP))
 827                 return;
 828
 829         printf("Echo rsp: dlen %d\n", btohs(cmd->len));
 830         raw_dump(level, frm);
 831 }
 832
 833 static void info_opt(int level, int type, void *ptr, int len)
 834 {
 835         uint32_t mask;
 836         uint64_t fc_mask;
 837         int i;
 838
 839         p_indent(level, 0);
 840
 841         switch (type) {
 842         case 0x0001:
 843                 printf("Connectionless MTU %d\n", get_val(ptr, len));
 844                 break;
 845         case 0x0002:
 846                 mask = get_val(ptr, len);
 847                 printf("Extended feature mask 0x%4.4x\n", mask);
 848                 if (parser.flags & DUMP_VERBOSE)
 849                         for (i=0; l2cap_features[i].name; i++)
 850                                 if (mask & l2cap_features[i].flag) {
 851                                         p_indent(level + 1, 0);
 852                                         printf("%s\n", l2cap_features[i].name);
 853                                 }
 854                 break;
 855         case 0x0003:
 856                 fc_mask = get_le64(ptr);
 857                 printf("Fixed channel list 0x%8.8" PRIx64 "\n", fc_mask);
 858                 if (parser.flags & DUMP_VERBOSE)
 859                         for (i=0; l2cap_fix_chan[i].name; i++)
 860                                 if (fc_mask & l2cap_fix_chan[i].flag) {
 861                                         p_indent(level + 1, 0);
 862                                         printf("%s\n", l2cap_fix_chan[i].name);
 863                                 }
 864                 break;
 865         default:
 866                 printf("Unknown (len %d)\n", len);
 867                 break;
 868         }
 869 }
 870
 871 static inline void info_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
 872 {
 873         l2cap_info_req *h = frm->ptr;
 874
 875         if (p_filter(FILT_L2CAP))
 876                 return;
 877
 878         printf("Info req: type %d\n", btohs(h->type));
 879 }
 880
 881 static inline void info_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
 882 {
 883         l2cap_info_rsp *h = frm->ptr;
 884         uint16_t type = btohs(h->type);
 885         uint16_t result = btohs(h->result);
 886         int ilen = btohs(cmd->len) - L2CAP_INFO_RSP_SIZE;
 887
 888         if (p_filter(FILT_L2CAP))
 889                 return;
 890
 891         printf("Info rsp: type %d result %d\n", type, result);
 892
 893         if (ilen > 0) {
 894                 info_opt(level + 1, type, h->data, ilen);
 895         } else {
 896                 p_indent(level + 1, frm);
 897                 printf("%s\n", inforesult2str(result));
 898         }
 899 }
 900
 901 static void l2cap_ctrl_ext_parse(int level, struct frame *frm, uint32_t ctrl)
 902 {
 903         p_indent(level, frm);
 904
 905         printf("%s:", ctrl & L2CAP_EXT_CTRL_FRAME_TYPE ? "S-frame" : "I-frame");
 906
 907         if (ctrl & L2CAP_EXT_CTRL_FRAME_TYPE) {
 908                 printf(" %s", supervisory2str((ctrl & L2CAP_EXT_CTRL_SUPERVISE_MASK) >>
 909                                         L2CAP_EXT_CTRL_SUPER_SHIFT));
 910
 911                 if (ctrl & L2CAP_EXT_CTRL_POLL)
 912                         printf(" P-bit");
 913         } else {
 914                 uint8_t sar = (ctrl & L2CAP_EXT_CTRL_SAR_MASK) >>
 915                         L2CAP_EXT_CTRL_SAR_SHIFT;
 916                 printf(" %s", sar2str(sar));
 917                 if (sar == L2CAP_SAR_START) {
 918                         uint16_t len;
 919                         len = get_le16(frm->ptr);
 920                         frm->ptr += L2CAP_SDULEN_SIZE;
 921                         frm->len -= L2CAP_SDULEN_SIZE;
 922                         printf(" (len %d)", len);
 923                 }
 924                 printf(" TxSeq %d", (ctrl & L2CAP_EXT_CTRL_TXSEQ_MASK) >>
 925                                 L2CAP_EXT_CTRL_TXSEQ_SHIFT);
 926         }
 927
 928         printf(" ReqSeq %d", (ctrl & L2CAP_EXT_CTRL_REQSEQ_MASK) >>
 929                         L2CAP_EXT_CTRL_REQSEQ_SHIFT);
 930
 931         if (ctrl & L2CAP_EXT_CTRL_FINAL)
 932                 printf(" F-bit");
 933 }
 934
 935 static void l2cap_ctrl_parse(int level, struct frame *frm, uint32_t ctrl)
 936 {
 937         p_indent(level, frm);
 938
 939         printf("%s:", ctrl & L2CAP_CTRL_FRAME_TYPE ? "S-frame" : "I-frame");
 940
 941         if (ctrl & 0x01) {
 942                 printf(" %s", supervisory2str((ctrl & L2CAP_CTRL_SUPERVISE_MASK) >>
 943                                         L2CAP_CTRL_SUPER_SHIFT));
 944
 945                 if (ctrl & L2CAP_CTRL_POLL)
 946                         printf(" P-bit");
 947         } else {
 948                 uint8_t sar = (ctrl & L2CAP_CTRL_SAR_MASK) >> L2CAP_CTRL_SAR_SHIFT;
 949                 printf(" %s", sar2str(sar));
 950                 if (sar == L2CAP_SAR_START) {
 951                         uint16_t len;
 952                         len = get_le16(frm->ptr);
 953                         frm->ptr += L2CAP_SDULEN_SIZE;
 954                         frm->len -= L2CAP_SDULEN_SIZE;
 955                         printf(" (len %d)", len);
 956                 }
 957                 printf(" TxSeq %d", (ctrl & L2CAP_CTRL_TXSEQ_MASK) >> L2CAP_CTRL_TXSEQ_SHIFT);
 958         }
 959
 960         printf(" ReqSeq %d", (ctrl & L2CAP_CTRL_REQSEQ_MASK) >> L2CAP_CTRL_REQSEQ_SHIFT);
 961
 962         if (ctrl & L2CAP_CTRL_FINAL)
 963                 printf(" F-bit");
 964 }
 965
 966 static inline void create_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
 967 {
 968         l2cap_create_req *h = frm->ptr;
 969         uint16_t psm = btohs(h->psm);
 970         uint16_t scid = btohs(h->scid);
 971
 972         if (p_filter(FILT_L2CAP))
 973                 return;
 974
 975         printf("Create chan req: psm 0x%4.4x scid 0x%4.4x ctrl id %d\n",
 976                                                         psm, scid, h->id);
 977 }
 978
 979 static inline void create_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
 980 {
 981         l2cap_create_rsp *h = frm->ptr;
 982         uint16_t scid = btohs(h->scid);
 983         uint16_t dcid = btohs(h->dcid);
 984         uint16_t result = btohs(h->result);
 985         uint16_t status = btohs(h->status);
 986
 987         if (p_filter(FILT_L2CAP))
 988                 return;
 989
 990         printf("Create chan rsp: dcid 0x%4.4x scid 0x%4.4x result %d status %d\n",
 991                                                 dcid, scid, result, status);
 992 }
 993
 994 static inline void move_req(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
 995 {
 996         l2cap_move_req *h = frm->ptr;
 997         uint16_t icid = btohs(h->icid);
 998
 999         if (p_filter(FILT_L2CAP))
1000                 return;
1001
1002         printf("Move chan req: icid 0x%4.4x ctrl id %d\n", icid, h->id);
1003 }
1004
1005 static inline void move_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
1006 {
1007         l2cap_move_rsp *h = frm->ptr;
1008         uint16_t icid = btohs(h->icid);
1009         uint16_t result = btohs(h->result);
1010
1011         if (p_filter(FILT_L2CAP))
1012                 return;
1013
1014         printf("Move chan rsp: icid 0x%4.4x result %d\n", icid, result);
1015 }
1016
1017 static inline void move_cfm(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
1018 {
1019         l2cap_move_cfm *h = frm->ptr;
1020         uint16_t icid = btohs(h->icid);
1021         uint16_t result = btohs(h->result);
1022
1023         if (p_filter(FILT_L2CAP))
1024                 return;
1025
1026         printf("Move chan cfm: icid 0x%4.4x result %d\n", icid, result);
1027 }
1028
1029 static inline void move_cfm_rsp(int level, l2cap_cmd_hdr *cmd, struct frame *frm)
1030 {
1031         l2cap_move_cfm_rsp *h = frm->ptr;
1032         uint16_t icid = btohs(h->icid);
1033
1034         if (p_filter(FILT_L2CAP))
1035                 return;
1036
1037         printf("Move chan cfm rsp: icid 0x%4.4x\n", icid);
1038 }
1039
1040 static inline void a2mp_command_rej(int level, struct frame *frm)
1041 {
1042         struct a2mp_command_rej *h = frm->ptr;
1043         uint16_t reason = btohs(h->reason);
1044
1045         printf("Command Reject: reason %d\n", reason);
1046         p_indent(level + 1, 0);
1047         printf("%s\n", a2mpreason2str(reason));
1048 }
1049
1050 static inline void a2mp_discover_req(int level, struct frame *frm, uint16_t len)
1051 {
1052         struct a2mp_discover_req *h = frm->ptr;
1053         uint16_t mtu = btohs(h->mtu);
1054         uint8_t  *octet = (uint8_t *)&(h->mask);
1055         uint16_t mask;
1056         uint8_t  extension;
1057
1058         printf("Discover req: mtu/mps %d ", mtu);
1059         len -= 2;
1060
1061         printf("mask:");
1062
1063         do {
1064                 len -= 2;
1065                 mask = get_le16(octet);
1066                 printf(" 0x%4.4x", mask);
1067
1068                 extension = octet[1] & 0x80;
1069                 octet += 2;
1070         } while ((extension != 0) && (len >= 2));
1071
1072         printf("\n");
1073 }
1074
1075 static inline void a2mp_ctrl_list_dump(int level, struct a2mp_ctrl *list, uint16_t len)
1076 {
1077         p_indent(level, 0);
1078         printf("Controller list:\n");
1079
1080         while (len >= 3) {
1081                 p_indent(level + 1, 0);
1082                 printf("id %d type %d (%s) status 0x%2.2x (%s)\n",
1083                            list->id, list->type, ampctrltype2str(list->type), list->status, ampctrlstatus2str(list->status));
1084                 list++;
1085                 len -= 3;
1086         }
1087
1088 }
1089
1090 static inline void a2mp_discover_rsp(int level, struct frame *frm, uint16_t len)
1091 {
1092         struct a2mp_discover_rsp *h = frm->ptr;
1093         uint16_t mtu = btohs(h->mtu);
1094         uint8_t  *octet = (uint8_t *)&(h->mask);
1095         uint16_t mask;
1096         uint8_t  extension;
1097
1098         printf("Discover rsp: mtu/mps %d ", mtu);
1099         len -= 2;
1100
1101         printf("mask:");
1102
1103         do {
1104                 len -= 2;
1105                 mask = get_le16(octet);
1106                 printf(" 0x%4.4x", mask);
1107
1108                 extension = octet[1] & 0x80;
1109                 octet += 2;
1110         } while ((extension != 0) && (len >= 2));
1111
1112         printf("\n");
1113
1114         if (len >= 3) {
1115                 a2mp_ctrl_list_dump(level + 1, (struct a2mp_ctrl *) octet, len);
1116         }
1117 }
1118
1119 static inline void a2mp_change_notify(int level, struct frame *frm, uint16_t len)
1120 {
1121         struct a2mp_ctrl *list = frm->ptr;
1122
1123         printf("Change Notify\n");
1124
1125         if (len >= 3) {
1126                 a2mp_ctrl_list_dump(level + 1, list, len);
1127         }
1128 }
1129
1130 static inline void a2mp_change_rsp(int level, struct frame *frm)
1131 {
1132         printf("Change Response\n");
1133 }
1134
1135 static inline void a2mp_info_req(int level, struct frame *frm)
1136 {
1137         struct a2mp_info_req *h = frm->ptr;
1138
1139         printf("Get Info req: id %d\n", h->id);
1140 }
1141
1142 static inline void a2mp_info_rsp(int level, struct frame *frm)
1143 {
1144         struct a2mp_info_rsp *h = frm->ptr;
1145
1146         printf("Get Info rsp: id %d status %d (%s)\n",
1147                    h->id, h->status, a2mpstatus2str(h->status));
1148
1149         p_indent(level + 1, 0);
1150         printf("Total bandwidth %d\n", btohl(h->total_bw));
1151         p_indent(level + 1, 0);
1152         printf("Max guaranteed bandwidth %d\n", btohl(h->max_bw));
1153         p_indent(level + 1, 0);
1154         printf("Min latency %d\n", btohl(h->min_latency));
1155         p_indent(level + 1, 0);
1156         printf("Pal capabilities 0x%4.4x\n", btohs(h->pal_caps));
1157         p_indent(level + 1, 0);
1158         printf("Assoc size %d\n", btohs(h->assoc_size));
1159 }
1160
1161 static inline void a2mp_assoc_req(int level, struct frame *frm)
1162 {
1163         struct a2mp_assoc_req *h = frm->ptr;
1164
1165         printf("Get AMP Assoc req: id %d\n", h->id);
1166 }
1167
1168 static inline void a2mp_assoc_rsp(int level, struct frame *frm, uint16_t len)
1169 {
1170         struct a2mp_assoc_rsp *h = frm->ptr;
1171
1172         printf("Get AMP Assoc rsp: id %d status (%d) %s\n",
1173                         h->id, h->status, a2mpstatus2str(h->status));
1174         amp_assoc_dump(level + 1, h->assoc_data, len - sizeof(*h));
1175 }
1176
1177 static inline void a2mp_create_req(int level, struct frame *frm, uint16_t len)
1178 {
1179         struct a2mp_create_req *h = frm->ptr;
1180
1181         printf("Create Physical Link req: local id %d remote id %d\n",
1182                    h->local_id, h->remote_id);
1183         amp_assoc_dump(level + 1, h->assoc_data, len - sizeof(*h));
1184 }
1185
1186 static inline void a2mp_create_rsp(int level, struct frame *frm)
1187 {
1188         struct a2mp_create_rsp *h = frm->ptr;
1189
1190         printf("Create Physical Link rsp: local id %d remote id %d status %d\n",
1191                    h->local_id, h->remote_id, h->status);
1192         p_indent(level+1, 0);
1193         printf("%s\n", a2mpcplstatus2str(h->status));
1194 }
1195
1196 static inline void a2mp_disconn_req(int level, struct frame *frm)
1197 {
1198         struct a2mp_disconn_req *h = frm->ptr;
1199
1200         printf("Disconnect Physical Link req: local id %d remote id %d\n",
1201                    h->local_id, h->remote_id);
1202 }
1203
1204 static inline void a2mp_disconn_rsp(int level, struct frame *frm)
1205 {
1206         struct a2mp_disconn_rsp *h = frm->ptr;
1207
1208         printf("Disconnect Physical Link rsp: local id %d remote id %d status %d\n",
1209                    h->local_id, h->remote_id, h->status);
1210         p_indent(level+1, 0);
1211         printf("%s\n", a2mpdplstatus2str(h->status));
1212 }
1213
1214 static void l2cap_parse(int level, struct frame *frm)
1215 {
1216         l2cap_hdr *hdr = (void *)frm->ptr;
1217         uint16_t dlen = btohs(hdr->len);
1218         uint16_t cid  = btohs(hdr->cid);
1219         uint16_t psm;
1220
1221         frm->ptr += L2CAP_HDR_SIZE;
1222         frm->len -= L2CAP_HDR_SIZE;
1223
1224         if (cid == 0x1) {
1225                 /* Signaling channel */
1226
1227                 while (frm->len >= L2CAP_CMD_HDR_SIZE) {
1228                         l2cap_cmd_hdr *hdr = frm->ptr;
1229
1230                         frm->ptr += L2CAP_CMD_HDR_SIZE;
1231                         frm->len -= L2CAP_CMD_HDR_SIZE;
1232
1233                         if (!p_filter(FILT_L2CAP)) {
1234                                 p_indent(level, frm);
1235                                 printf("L2CAP(s): ");
1236                         }
1237
1238                         switch (hdr->code) {
1239                         case L2CAP_COMMAND_REJ:
1240                                 command_rej(level, frm);
1241                                 break;
1242
1243                         case L2CAP_CONN_REQ:
1244                                 conn_req(level, frm);
1245                                 break;
1246
1247                         case L2CAP_CONN_RSP:
1248                                 conn_rsp(level, frm);
1249                                 break;
1250
1251                         case L2CAP_CONF_REQ:
1252                                 conf_req(level, hdr, frm);
1253                                 break;
1254
1255                         case L2CAP_CONF_RSP:
1256                                 conf_rsp(level, hdr, frm);
1257                                 break;
1258
1259                         case L2CAP_DISCONN_REQ:
1260                                 disconn_req(level, frm);
1261                                 break;
1262
1263                         case L2CAP_DISCONN_RSP:
1264                                 disconn_rsp(level, frm);
1265                                 break;
1266
1267                         case L2CAP_ECHO_REQ:
1268                                 echo_req(level, hdr, frm);
1269                                 break;
1270
1271                         case L2CAP_ECHO_RSP:
1272                                 echo_rsp(level, hdr, frm);
1273                                 break;
1274
1275                         case L2CAP_INFO_REQ:
1276                                 info_req(level, hdr, frm);
1277                                 break;
1278
1279                         case L2CAP_INFO_RSP:
1280                                 info_rsp(level, hdr, frm);
1281                                 break;
1282
1283                         case L2CAP_CREATE_REQ:
1284                                 create_req(level, hdr, frm);
1285                                 break;
1286
1287                         case L2CAP_CREATE_RSP:
1288                                 create_rsp(level, hdr, frm);
1289                                 break;
1290
1291                         case L2CAP_MOVE_REQ:
1292                                 move_req(level, hdr, frm);
1293                                 break;
1294
1295                         case L2CAP_MOVE_RSP:
1296                                 move_rsp(level, hdr, frm);
1297                                 break;
1298
1299                         case L2CAP_MOVE_CFM:
1300                                 move_cfm(level, hdr, frm);
1301                                 break;
1302
1303                         case L2CAP_MOVE_CFM_RSP:
1304                                 move_cfm_rsp(level, hdr, frm);
1305                                 break;
1306
1307                         default:
1308                                 if (p_filter(FILT_L2CAP))
1309                                         break;
1310                                 printf("code 0x%2.2x ident %d len %d\n",
1311                                         hdr->code, hdr->ident, btohs(hdr->len));
1312                                 raw_dump(level, frm);
1313                         }
1314
1315                         if (frm->len > btohs(hdr->len)) {
1316                                 frm->len -= btohs(hdr->len);
1317                                 frm->ptr += btohs(hdr->len);
1318                         } else
1319                                 frm->len = 0;
1320                 }
1321         } else if (cid == 0x2) {
1322                 /* Connectionless channel */
1323
1324                 if (p_filter(FILT_L2CAP))
1325                         return;
1326
1327                 psm = get_le16(frm->ptr);
1328                 frm->ptr += 2;
1329                 frm->len -= 2;
1330
1331                 p_indent(level, frm);
1332                 printf("L2CAP(c): len %d psm %d\n", dlen, psm);
1333                 raw_dump(level, frm);
1334         } else if (cid == 0x3) {
1335                 /* AMP Manager channel */
1336
1337                 if (p_filter(FILT_A2MP))
1338                         return;
1339
1340                 /* Adjust for ERTM control bytes */
1341                 frm->ptr += 2;
1342                 frm->len -= 2;
1343
1344                 while (frm->len >= A2MP_HDR_SIZE) {
1345                         struct a2mp_hdr *hdr = frm->ptr;
1346
1347                         frm->ptr += A2MP_HDR_SIZE;
1348                         frm->len -= A2MP_HDR_SIZE;
1349
1350                         p_indent(level, frm);
1351                         printf("A2MP: ");
1352
1353                         switch (hdr->code) {
1354                         case A2MP_COMMAND_REJ:
1355                                 a2mp_command_rej(level, frm);
1356                                 break;
1357                         case A2MP_DISCOVER_REQ:
1358                                 a2mp_discover_req(level, frm, btohs(hdr->len));
1359                                 break;
1360                         case A2MP_DISCOVER_RSP:
1361                                 a2mp_discover_rsp(level, frm, btohs(hdr->len));
1362                                 break;
1363                         case A2MP_CHANGE_NOTIFY:
1364                                 a2mp_change_notify(level, frm, btohs(hdr->len));
1365                                 break;
1366                         case A2MP_CHANGE_RSP:
1367                                 a2mp_change_rsp(level, frm);
1368                                 break;
1369                         case A2MP_INFO_REQ:
1370                                 a2mp_info_req(level, frm);
1371                                 break;
1372                         case A2MP_INFO_RSP:
1373                                 a2mp_info_rsp(level, frm);
1374                                 break;
1375                         case A2MP_ASSOC_REQ:
1376                                 a2mp_assoc_req(level, frm);
1377                                 break;
1378                         case A2MP_ASSOC_RSP:
1379                                 a2mp_assoc_rsp(level, frm, btohs(hdr->len));
1380                                 break;
1381                         case A2MP_CREATE_REQ:
1382                                 a2mp_create_req(level, frm, btohs(hdr->len));
1383                                 break;
1384                         case A2MP_CREATE_RSP:
1385                                 a2mp_create_rsp(level, frm);
1386                                 break;
1387                         case A2MP_DISCONN_REQ:
1388                                 a2mp_disconn_req(level, frm);
1389                                 break;
1390                         case A2MP_DISCONN_RSP:
1391                                 a2mp_disconn_rsp(level, frm);
1392                                 break;
1393                         default:
1394                                 printf("code 0x%2.2x ident %d len %d\n",
1395                                            hdr->code, hdr->ident, btohs(hdr->len));
1396                                 raw_dump(level, frm);
1397                         }
1398                         if (frm->len > btohs(hdr->len)) {
1399                                 frm->len -= btohs(hdr->len);
1400                                 frm->ptr += btohs(hdr->len);
1401                         } else
1402                                 frm->len = 0;
1403                 }
1404         } else if (cid == 0x04) {
1405                 if (!p_filter(FILT_ATT))
1406                         att_dump(level, frm);
1407                 else
1408                         raw_dump(level + 1, frm);
1409         } else if (cid == 0x06) {
1410                 if (!p_filter(FILT_SMP))
1411                         smp_dump(level, frm);
1412                 else
1413                         raw_dump(level + 1, frm);
1414         } else {
1415                 /* Connection oriented channel */
1416
1417                 uint8_t mode = get_mode(!frm->in, frm->handle, cid);
1418                 uint8_t ext_ctrl = get_ext_ctrl(!frm->in, frm->handle, cid);
1419                 uint16_t psm = get_psm(!frm->in, frm->handle, cid);
1420                 uint16_t fcs = 0;
1421                 uint32_t proto, ctrl = 0;
1422
1423                 frm->cid = cid;
1424                 frm->num = get_num(!frm->in, frm->handle, cid);
1425
1426                 if (mode > 0) {
1427                         if (ext_ctrl) {
1428                                 ctrl = get_val(frm->ptr, 4);
1429                                 frm->ptr += 4;
1430                                 frm->len -= 6;
1431                         } else {
1432                                 ctrl = get_val(frm->ptr, 2);
1433                                 frm->ptr += 2;
1434                                 frm->len -= 4;
1435                         }
1436                         fcs = get_le16(frm->ptr + frm->len);
1437                 }
1438
1439                 if (!p_filter(FILT_L2CAP)) {
1440                         p_indent(level, frm);
1441                         printf("L2CAP(d): cid 0x%4.4x len %d", cid, dlen);
1442                         if (mode > 0) {
1443                                 if (ext_ctrl)
1444                                         printf(" ext_ctrl 0x%8.8x fcs 0x%4.4x", ctrl, fcs);
1445                                 else
1446                                         printf(" ctrl 0x%4.4x fcs 0x%4.4x", ctrl, fcs);
1447                         }
1448
1449                         printf(" [psm %d]\n", psm);
1450                         level++;
1451                         if (mode > 0) {
1452                                 if (ext_ctrl)
1453                                         l2cap_ctrl_ext_parse(level, frm, ctrl);
1454                                 else
1455                                         l2cap_ctrl_parse(level, frm, ctrl);
1456
1457                                 printf("\n");
1458                         }
1459                 }
1460
1461                 switch (psm) {
1462                 case 0x01:
1463                         if (!p_filter(FILT_SDP))
1464                                 sdp_dump(level + 1, frm);
1465                         else
1466                                 raw_dump(level + 1, frm);
1467                         break;
1468
1469                 case 0x03:
1470                         if (!p_filter(FILT_RFCOMM))
1471                                 rfcomm_dump(level, frm);
1472                         else
1473                                 raw_dump(level + 1, frm);
1474                         break;
1475
1476                 case 0x0f:
1477                         if (!p_filter(FILT_BNEP))
1478                                 bnep_dump(level, frm);
1479                         else
1480                                 raw_dump(level + 1, frm);
1481                         break;
1482
1483                 case 0x11:
1484                 case 0x13:
1485                         if (!p_filter(FILT_HIDP))
1486                                 hidp_dump(level, frm);
1487                         else
1488                                 raw_dump(level + 1, frm);
1489                         break;
1490
1491                 case 0x17:
1492                 case 0x1B:
1493                         if (!p_filter(FILT_AVCTP))
1494                                 avctp_dump(level, frm, psm);
1495                         else
1496                                 raw_dump(level + 1, frm);
1497                         break;
1498
1499                 case 0x19:
1500                         if (!p_filter(FILT_AVDTP))
1501                                 avdtp_dump(level, frm);
1502                         else
1503                                 raw_dump(level + 1, frm);
1504                         break;
1505
1506                 case 0x1f:
1507                         if (!p_filter(FILT_ATT))
1508                                 att_dump(level, frm);
1509                         else
1510                                 raw_dump(level + 1, frm);
1511                         break;
1512
1513                 default:
1514                         proto = get_proto(frm->handle, psm, 0);
1515
1516                         switch (proto) {
1517                         case SDP_UUID_CMTP:
1518                                 if (!p_filter(FILT_CMTP))
1519                                         cmtp_dump(level, frm);
1520                                 else
1521                                         raw_dump(level + 1, frm);
1522                                 break;
1523
1524                         case SDP_UUID_HARDCOPY_CONTROL_CHANNEL:
1525                                 if (!p_filter(FILT_HCRP))
1526                                         hcrp_dump(level, frm);
1527                                 else
1528                                         raw_dump(level + 1, frm);
1529                                 break;
1530
1531                         case SDP_UUID_OBEX:
1532                                 if (!p_filter(FILT_OBEX))
1533                                         obex_dump(level, frm);
1534                                 else
1535                                         raw_dump(level + 1, frm);
1536                                 break;
1537
1538                         default:
1539                                 if (p_filter(FILT_L2CAP))
1540                                         break;
1541
1542                                 raw_dump(level, frm);
1543                                 break;
1544                         }
1545                         break;
1546                 }
1547         }
1548 }
1549
1550 void l2cap_dump(int level, struct frame *frm)
1551 {
1552         struct frame *fr;
1553         l2cap_hdr *hdr;
1554         uint16_t dlen;
1555
1556         if ((frm->flags & ACL_START) || frm->flags == ACL_START_NO_FLUSH) {
1557                 hdr  = frm->ptr;
1558                 dlen = btohs(hdr->len);
1559
1560                 if (dlen + L2CAP_HDR_SIZE < (int) frm->len) {
1561                         /* invalid frame */
1562                         raw_dump(level,frm);
1563                         return;
1564                 }
1565
1566                 if ((int) frm->len == (dlen + L2CAP_HDR_SIZE)) {
1567                         /* Complete frame */
1568                         l2cap_parse(level, frm);
1569                         return;
1570                 }
1571
1572                 if (!(fr = get_frame(frm->handle))) {
1573                         fprintf(stderr, "Not enough connection handles\n");
1574                         raw_dump(level, frm);
1575                         return;
1576                 }
1577
1578                 if (fr->data)
1579                         free(fr->data);
1580
1581                 if (!(fr->data = malloc(dlen + L2CAP_HDR_SIZE))) {
1582                         perror("Can't allocate L2CAP reassembly buffer");
1583                         return;
1584                 }
1585                 memcpy(fr->data, frm->ptr, frm->len);
1586                 fr->data_len   = dlen + L2CAP_HDR_SIZE;
1587                 fr->len        = frm->len;
1588                 fr->ptr        = fr->data;
1589                 fr->dev_id     = frm->dev_id;
1590                 fr->in         = frm->in;
1591                 fr->ts         = frm->ts;
1592                 fr->handle     = frm->handle;
1593                 fr->cid        = frm->cid;
1594                 fr->num        = frm->num;
1595                 fr->dlci       = frm->dlci;
1596                 fr->channel    = frm->channel;
1597                 fr->pppdump_fd = frm->pppdump_fd;
1598                 fr->audio_fd   = frm->audio_fd;
1599         } else {
1600                 if (!(fr = get_frame(frm->handle))) {
1601                         fprintf(stderr, "Not enough connection handles\n");
1602                         raw_dump(level, frm);
1603                         return;
1604                 }
1605
1606                 if (!fr->data) {
1607                         /* Unexpected fragment */
1608                         raw_dump(level, frm);
1609                         return;
1610                 }
1611
1612                 if (frm->len > (fr->data_len - fr->len)) {
1613                         /* Bad fragment */
1614                         raw_dump(level, frm);
1615                         free(fr->data); fr->data = NULL;
1616                         return;
1617                 }
1618
1619                 memcpy(fr->data + fr->len, frm->ptr, frm->len);
1620                 fr->len += frm->len;
1621
1622                 if (fr->len == fr->data_len) {
1623                         /* Complete frame */
1624                         l2cap_parse(level, fr);
1625
1626                         free(fr->data); fr->data = NULL;
1627                         return;
1628                 }
1629         }
1630 }
1631
1632 void l2cap_clear(uint16_t handle)
1633 {
1634         del_handle(handle);
1635 }