static inline void conn_rsp(int level, struct frame *frm) { l2cap_conn_rsp *h = frm->ptr; uint16_t scid = btohs(h->scid); uint16_t dcid = btohs(h->dcid); uint16_t result = btohs(h->result); uint16_t status = btohs(h->status); uint16_t psm; switch (h->result) { case L2CAP_CR_SUCCESS: if ((psm = get_psm(!frm->in, frm->handle, scid))) add_cid(frm->in, frm->handle, dcid, psm); break; case L2CAP_CR_PEND: break; default: del_cid(frm->in, dcid, scid); break; } if (p_filter(FILT_L2CAP)) return; printf("Connect rsp: dcid 0x%4.4x scid 0x%4.4x result %d status %d\n", dcid, scid, result, status); p_indent(level + 1, frm); printf("%s", connresult2str(result)); if (result == 0x0001) printf(" - %s\n", status2str(status)); else printf("\n"); }
static void l2cap_parse(int level, struct frame *frm) { l2cap_hdr *hdr = (void *)frm->ptr; uint16_t dlen = btohs(hdr->len); uint16_t cid = btohs(hdr->cid); uint16_t psm; frm->ptr += L2CAP_HDR_SIZE; frm->len -= L2CAP_HDR_SIZE; if (cid == 0x1) { /* Signaling channel */ while (frm->len >= L2CAP_CMD_HDR_SIZE) { l2cap_cmd_hdr *hdr = frm->ptr; frm->ptr += L2CAP_CMD_HDR_SIZE; frm->len -= L2CAP_CMD_HDR_SIZE; if (!p_filter(FILT_L2CAP)) { p_indent(level, frm); printf("L2CAP(s): "); } switch (hdr->code) { case L2CAP_COMMAND_REJ: command_rej(level, frm); break; case L2CAP_CONN_REQ: conn_req(level, frm); break; case L2CAP_CONN_RSP: conn_rsp(level, frm); break; case L2CAP_CONF_REQ: conf_req(level, hdr, frm); break; case L2CAP_CONF_RSP: conf_rsp(level, hdr, frm); break; case L2CAP_DISCONN_REQ: disconn_req(level, frm); break; case L2CAP_DISCONN_RSP: disconn_rsp(level, frm); break; case L2CAP_ECHO_REQ: echo_req(level, hdr, frm); break; case L2CAP_ECHO_RSP: echo_rsp(level, hdr, frm); break; case L2CAP_INFO_REQ: info_req(level, hdr, frm); break; case L2CAP_INFO_RSP: info_rsp(level, hdr, frm); break; default: if (p_filter(FILT_L2CAP)) break; printf("code 0x%2.2x ident %d len %d\n", hdr->code, hdr->ident, btohs(hdr->len)); raw_dump(level, frm); } if (frm->len > btohs(hdr->len)) { frm->len -= btohs(hdr->len); frm->ptr += btohs(hdr->len); } else frm->len = 0; } } else if (cid == 0x2) { /* Connectionless channel */ if (p_filter(FILT_L2CAP)) return; psm = btohs(bt_get_unaligned((uint16_t *) frm->ptr)); frm->ptr += 2; frm->len -= 2; p_indent(level, frm); printf("L2CAP(c): len %d psm %d\n", dlen, psm); raw_dump(level, frm); } else { /* Connection oriented channel */ uint8_t mode = get_mode(!frm->in, cid); uint16_t psm = get_psm(!frm->in, cid); uint16_t ctrl = 0, fcs = 0; uint32_t proto; frm->cid = cid; frm->num = get_num(!frm->in, cid); if (mode > 0) { ctrl = btohs(bt_get_unaligned((uint16_t *) frm->ptr)); frm->ptr += 2; frm->len -= 4; fcs = btohs(bt_get_unaligned((uint16_t *) (frm->ptr + frm->len))); } if (!p_filter(FILT_L2CAP)) { p_indent(level, frm); printf("L2CAP(d): cid 0x%4.4x len %d", cid, dlen); if (mode > 0) printf(" ctrl 0x%4.4x fcs 0x%4.4x", ctrl, fcs); printf(" [psm %d]\n", psm); level++; if (mode > 0) { p_indent(level, frm); printf("%s:", ctrl & 0x01 ? "S-frame" : "I-frame"); if (ctrl & 0x01) { printf(" %s", supervisory2str((ctrl & 0x0c) >> 2)); } else {