static bool setup_conn_req(struct bnep_frame *bnep_frame, uint8_t indent) { struct l2cap_frame *frame = &bnep_frame->l2cap_frame; uint8_t uuid_size; uint32_t src_uuid = 0, dst_uuid = 0; if (!l2cap_frame_get_u8(frame, &uuid_size)) return false; print_field("%*cSize: 0x%02x ", indent, ' ', uuid_size); switch (uuid_size) { case 2: if (!l2cap_frame_get_be16(frame, (uint16_t *) &dst_uuid)) return false; if (!l2cap_frame_get_be16(frame, (uint16_t *) &src_uuid)) return false; break; case 4: if (!l2cap_frame_get_be32(frame, &dst_uuid)) return false; if (!l2cap_frame_get_be32(frame, &src_uuid)) return false; break; case 16: if (!l2cap_frame_get_be32(frame, &dst_uuid)) return false; l2cap_frame_pull(frame, frame, 12); if (!l2cap_frame_get_be32(frame, &src_uuid)) return false; l2cap_frame_pull(frame, frame, 12); break; default: l2cap_frame_pull(frame, frame, (uuid_size * 2)); return true; } print_field("%*cDst: 0x%x(%s)", indent, ' ', dst_uuid, bt_uuid32_to_str(dst_uuid)); print_field("%*cSrc: 0x%x(%s)", indent, ' ', src_uuid, bt_uuid32_to_str(src_uuid)); return true; }
void avctp_packet(const struct l2cap_frame *frame) { struct l2cap_frame *l2cap_frame; struct avctp_frame avctp_frame; const char *pdu_color; l2cap_frame_pull(&avctp_frame.l2cap_frame, frame, 0); l2cap_frame = &avctp_frame.l2cap_frame; if (!l2cap_frame_get_u8(l2cap_frame, &avctp_frame.hdr) || !l2cap_frame_get_be16(l2cap_frame, &avctp_frame.pid)) { print_text(COLOR_ERROR, "frame too short"); packet_hexdump(frame->data, frame->size); return; } if (frame->in) pdu_color = COLOR_MAGENTA; else pdu_color = COLOR_BLUE; print_indent(6, pdu_color, "AVCTP", "", COLOR_OFF, " %s: %s: type 0x%02x label %d PID 0x%04x", frame->psm == 23 ? "Control" : "Browsing", avctp_frame.hdr & 0x02 ? "Response" : "Command", avctp_frame.hdr & 0x0c, avctp_frame.hdr >> 4, avctp_frame.pid); if (avctp_frame.pid == 0x110e || avctp_frame.pid == 0x110c) avrcp_packet(&avctp_frame); else packet_hexdump(frame->data, frame->size); }
void sdp_packet(const struct l2cap_frame *frame) { uint8_t pdu; uint16_t tid, plen; struct l2cap_frame sdp_frame; struct tid_data *tid_info; const struct sdp_data *sdp_data = NULL; const char *pdu_color, *pdu_str; int i; l2cap_frame_pull(&sdp_frame, frame, 0); if (!l2cap_frame_get_u8(&sdp_frame, &pdu) || !l2cap_frame_get_be16(&sdp_frame, &tid) || !l2cap_frame_get_be16(&sdp_frame, &plen)) { print_text(COLOR_ERROR, "frame too short"); packet_hexdump(frame->data, frame->size); return; } if (sdp_frame.size != plen) { print_text(COLOR_ERROR, "invalid frame size"); packet_hexdump(sdp_frame.data, sdp_frame.size); return; } for (i = 0; sdp_table[i].str; i++) { if (sdp_table[i].pdu == pdu) { sdp_data = &sdp_table[i]; break; } } if (sdp_data) { if (sdp_data->func) { if (frame->in) pdu_color = COLOR_MAGENTA; else pdu_color = COLOR_BLUE; } else pdu_color = COLOR_WHITE_BG; pdu_str = sdp_data->str; } else { pdu_color = COLOR_WHITE_BG; pdu_str = "Unknown"; } print_indent(6, pdu_color, "SDP: ", pdu_str, COLOR_OFF, " (0x%2.2x) tid %d len %d", pdu, tid, plen); tid_info = get_tid(tid, frame->chan); if (!sdp_data || !sdp_data->func || !tid_info) { packet_hexdump(sdp_frame.data, sdp_frame.size); return; } sdp_data->func(&sdp_frame, tid_info); }
static bool bnep_control(struct bnep_frame *bnep_frame, uint8_t indent, int hdr_len) { uint8_t ctype; struct l2cap_frame *frame = &bnep_frame->l2cap_frame; const struct bnep_control_data *bnep_control_data = NULL; const char *type_str; int i; if (!l2cap_frame_get_u8(frame, &ctype)) return false; for (i = 0; bnep_control_table[i].str; i++) { if (bnep_control_table[i].type == ctype) { bnep_control_data = &bnep_control_table[i]; break; } } if (bnep_control_data) type_str = bnep_control_data->str; else type_str = "Unknown control type"; print_field("%*c%s (0x%02x) ", indent, ' ', type_str, ctype); if (!bnep_control_data || !bnep_control_data->func) { packet_hexdump(frame->data, hdr_len - 1); l2cap_frame_pull(frame, frame, hdr_len - 1); goto done; } if (!bnep_control_data->func(bnep_frame, indent+2)) return false; done: return true; }
static bool bnep_eval_extension(struct bnep_frame *bnep_frame, uint8_t indent) { struct l2cap_frame *frame = &bnep_frame->l2cap_frame; uint8_t type, length; int extension; if (!l2cap_frame_get_u8(frame, &type)) return false; if (!l2cap_frame_get_u8(frame, &length)) return false; extension = GET_EXTENSION(type); type = GET_PKT_TYPE(type); switch (type) { case BNEP_EXTENSION_CONTROL: print_field("%*cExt Control(0x%02x|%s) len 0x%02x", indent, ' ', type, extension ? "1" : "0", length); if (!bnep_control(bnep_frame, indent+2, length)) return false; break; default: print_field("%*cExt Unknown(0x%02x|%s) len 0x%02x", indent, ' ', type, extension ? "1" : "0", length); packet_hexdump(frame->data, length); l2cap_frame_pull(frame, frame, length); } if (extension) if (!bnep_eval_extension(bnep_frame, indent)) return false; return true; }
void rfcomm_packet(const struct l2cap_frame *frame) { uint8_t ctype, length, ex_length, indent = 1; const char *frame_str, *frame_color; struct l2cap_frame *l2cap_frame, tmp_frame; struct rfcomm_frame rfcomm_frame; struct rfcomm_lhdr hdr; const struct rfcomm_data *rfcomm_data = NULL; int i; l2cap_frame_pull(&rfcomm_frame.l2cap_frame, frame, 0); l2cap_frame = &rfcomm_frame.l2cap_frame; if (frame->size < 4) goto fail; if (!l2cap_frame_get_u8(l2cap_frame, &hdr.address) || !l2cap_frame_get_u8(l2cap_frame, &hdr.control) || !l2cap_frame_get_u8(l2cap_frame, &length)) goto fail; /* length maybe 1 or 2 octets */ if (RFCOMM_TEST_EA(length)) hdr.length = (uint16_t) GET_LEN8(length); else { if (!l2cap_frame_get_u8(l2cap_frame, &ex_length)) goto fail; hdr.length = ((uint16_t)length << 8) | ex_length; hdr.length = GET_LEN16(hdr.length); } l2cap_frame_pull(&tmp_frame, l2cap_frame, l2cap_frame->size-1); if(!l2cap_frame_get_u8(&tmp_frame, &hdr.fcs)) goto fail; /* Decoding frame type */ ctype = RFCOMM_GET_TYPE(hdr.control); for (i = 0; rfcomm_table[i].str; i++) { if (rfcomm_table[i].frame == ctype) { rfcomm_data = &rfcomm_table[i]; break; } } if (rfcomm_data) { if (frame->in) frame_color = COLOR_MAGENTA; else frame_color = COLOR_BLUE; frame_str = rfcomm_data->str; } else { frame_color = COLOR_WHITE_BG; frame_str = "Unknown"; } if (!rfcomm_data) { packet_hexdump(frame->data, frame->size); return; } print_indent(6, frame_color, "RFCOMM: ", frame_str, COLOR_OFF, "(0x%2.2x)", ctype); rfcomm_frame.hdr = hdr; print_rfcomm_hdr(&rfcomm_frame, indent); /* UIH frame */ if (ctype == 0xef) if (!uih_frame(&rfcomm_frame, indent)) goto fail; return; fail: print_text(COLOR_ERROR, "Frame too short"); packet_hexdump(frame->data, frame->size); return; }
void bnep_packet(const struct l2cap_frame *frame) { uint8_t type, indent = 1; struct bnep_frame bnep_frame; struct l2cap_frame *l2cap_frame; const struct bnep_data *bnep_data = NULL; const char *pdu_color, *pdu_str; int i; l2cap_frame_pull(&bnep_frame.l2cap_frame, frame, 0); l2cap_frame = &bnep_frame.l2cap_frame; if (!l2cap_frame_get_u8(l2cap_frame, &type)) goto fail; bnep_frame.extension = GET_EXTENSION(type); bnep_frame.type = GET_PKT_TYPE(type); for (i = 0; bnep_table[i].str; i++) { if (bnep_table[i].type == bnep_frame.type) { bnep_data = &bnep_table[i]; break; } } if (bnep_data) { if (bnep_data->func) { if (frame->in) pdu_color = COLOR_MAGENTA; else pdu_color = COLOR_BLUE; } else pdu_color = COLOR_WHITE_BG; pdu_str = bnep_data->str; } else { pdu_color = COLOR_WHITE_BG; pdu_str = "Unknown packet type"; } print_indent(6, pdu_color, "BNEP: ", pdu_str, COLOR_OFF, " (0x%02x|%s)", bnep_frame.type, bnep_frame.extension ? "1" : "0"); if (!bnep_data || !bnep_data->func) { packet_hexdump(l2cap_frame->data, l2cap_frame->size); return; } if (!bnep_data->func(&bnep_frame, indent, -1)) goto fail; /* Extension info */ if (bnep_frame.extension) if (!bnep_eval_extension(&bnep_frame, indent+2)) goto fail; /* Control packet => No payload info */ if (bnep_frame.type == BNEP_CONTROL) return; /* TODO: Handle BNEP IP packet */ packet_hexdump(l2cap_frame->data, l2cap_frame->size); return; fail: print_text(COLOR_ERROR, "frame too short"); packet_hexdump(frame->data, frame->size); }