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 {
