From 793cc67596cc77186dc40cb87caf470d216ff959 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Sat, 17 Nov 2012 01:52:12 +0900 Subject: [PATCH] monitor: Add basic decoding for SDP transactions --- Makefile.tools | 3 +- monitor/bt.h | 6 ++ monitor/l2cap.c | 35 +++---- monitor/l2cap.h | 32 ++++++ monitor/sdp.c | 268 ++++++++++++++++++++++++++++++++++++++++++++++++ monitor/sdp.h | 25 +++++ 6 files changed, 345 insertions(+), 24 deletions(-) create mode 100644 monitor/sdp.c create mode 100644 monitor/sdp.h diff --git a/Makefile.tools b/Makefile.tools index 717d2ad2e..cf87ef530 100644 --- a/Makefile.tools +++ b/Makefile.tools @@ -55,7 +55,8 @@ monitor_btmon_SOURCES = monitor/main.c monitor/bt.h \ monitor/btsnoop.h monitor/btsnoop.c \ monitor/control.h monitor/control.c \ monitor/packet.h monitor/packet.c \ - monitor/l2cap.h monitor/l2cap.c + monitor/l2cap.h monitor/l2cap.c \ + monitor/sdp.h monitor/sdp.c monitor_btmon_LDADD = lib/libbluetooth-private.la emulator_btvirt_SOURCES = emulator/main.c monitor/bt.h \ diff --git a/monitor/bt.h b/monitor/bt.h index 939fda73c..023643e52 100644 --- a/monitor/bt.h +++ b/monitor/bt.h @@ -1502,3 +1502,9 @@ struct bt_l2cap_pdu_conn_param_req { struct bt_l2cap_pdu_conn_param_rsp { uint16_t result; } __attribute__ ((packed)); + +struct bt_sdp_hdr { + uint8_t pdu; + uint16_t tid; + uint16_t plen; +} __attribute__ ((packed)); diff --git a/monitor/l2cap.c b/monitor/l2cap.c index 4118f51b9..08211a08a 100644 --- a/monitor/l2cap.c +++ b/monitor/l2cap.c @@ -26,8 +26,9 @@ #include #endif -#include +#include #include +#include #include @@ -35,27 +36,7 @@ #include "packet.h" #include "display.h" #include "l2cap.h" - -struct l2cap_frame { - uint16_t index; - bool in; - uint16_t handle; - uint16_t cid; - const void *data; - uint16_t size; -}; - -static inline void l2cap_frame_init(struct l2cap_frame *frame, - uint16_t index, bool in, uint16_t handle, - uint16_t cid, const void *data, uint16_t size) -{ - frame->index = index; - frame->in = in; - frame->handle = handle; - frame->cid = cid; - frame->data = data; - frame->size = size; -} +#include "sdp.h" #define MAX_CHAN 64 @@ -1357,7 +1338,15 @@ static void l2cap_frame(uint16_t index, bool in, uint16_t handle, print_indent(6, COLOR_CYAN, "Channel:", "", COLOR_OFF, " %d len %d [PSM %d mode %d]", cid, size, psm, mode); - packet_hexdump(data, size); + + switch (psm) { + case 0x0001: + sdp_packet(&frame); + break; + default: + packet_hexdump(data, size); + break; + } break; } } diff --git a/monitor/l2cap.h b/monitor/l2cap.h index 503bacaf9..30bcb6a5e 100644 --- a/monitor/l2cap.h +++ b/monitor/l2cap.h @@ -25,5 +25,37 @@ #include #include +struct l2cap_frame { + uint16_t index; + bool in; + uint16_t handle; + uint16_t cid; + const void *data; + uint16_t size; +}; + +static inline void l2cap_frame_init(struct l2cap_frame *frame, + uint16_t index, bool in, uint16_t handle, + uint16_t cid, const void *data, uint16_t size) +{ + frame->index = index; + frame->in = in; + frame->handle = handle; + frame->cid = cid; + frame->data = data; + frame->size = size; +} + +static inline void l2cap_frame_pull(struct l2cap_frame *frame, + const struct l2cap_frame *source, uint16_t len) +{ + frame->index = source->index; + frame->in = source->in; + frame->handle = source->handle; + frame->cid = source->cid; + frame->data = source->data + len; + frame->size = source->size - len; +} + void l2cap_packet(uint16_t index, bool in, uint16_t handle, uint8_t flags, const void *data, uint16_t size); diff --git a/monitor/sdp.c b/monitor/sdp.c new file mode 100644 index 000000000..204041011 --- /dev/null +++ b/monitor/sdp.c @@ -0,0 +1,268 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include + +#include + +#include "bt.h" +#include "packet.h" +#include "display.h" +#include "l2cap.h" +#include "sdp.h" + +static void print_continuation(const uint8_t *data, uint16_t size) +{ + if (data[0] != size - 1) { + print_text(COLOR_ERROR, "invalid continuation state"); + packet_hexdump(data, size); + return; + } + + print_field("Continuation state: %d", data[0]); + packet_hexdump(data + 1, size - 1); +} + +static uint16_t get_bytes(const uint8_t *data, uint16_t size) +{ + switch (data[0] & 0x07) { + case 5: + return 2 + data[1]; + case 6: + return 3 + bt_get_be16(data + 1); + case 7: + return 5 + bt_get_be32(data + 1); + } + + return 0; +} + +static void error_rsp(const struct l2cap_frame *frame) +{ + uint16_t error; + + if (frame->size < 2) { + print_text(COLOR_ERROR, "invalid size"); + packet_hexdump(frame->data, frame->size); + return; + } + + error = bt_get_be16(frame->data); + + print_field("Error code: 0x%2.2x", error); +} + +static void service_req(const struct l2cap_frame *frame) +{ + uint16_t search_bytes; + + search_bytes = get_bytes(frame->data, frame->size); + + print_field("Search pattern: [len %d]", search_bytes); + packet_hexdump(frame->data, search_bytes); + + print_field("Max record count: %d", + bt_get_be16(frame->data + search_bytes)); + + print_continuation(frame->data + search_bytes + 2, + frame->size - search_bytes - 2); +} + +static void service_rsp(const struct l2cap_frame *frame) +{ + uint16_t count; + int i; + + if (frame->size < 4) { + print_text(COLOR_ERROR, "invalid size"); + packet_hexdump(frame->data, frame->size); + return; + } + + count = bt_get_be16(frame->data + 2); + + print_field("Total record count: %d", bt_get_be16(frame->data)); + print_field("Current record count: %d", count); + + for (i = 0; i < count; i++) + print_field("Record handle: 0x%4.4x", + bt_get_be32(frame->data + 4 + (i * 4))); + + print_continuation(frame->data + 4 + (count * 4), + frame->size - 4 - (count * 4)); +} + +static void attr_req(const struct l2cap_frame *frame) +{ + if (frame->size < 6) { + print_text(COLOR_ERROR, "invalid size"); + packet_hexdump(frame->data, frame->size); + return; + } + + print_field("Record handle: 0x%4.4x", bt_get_be32(frame->data)); + print_field("Max attribute bytes: %d", bt_get_be16(frame->data + 4)); + + packet_hexdump(frame->data + 6, frame->size - 6); +} + +static void attr_rsp(const struct l2cap_frame *frame) +{ + uint16_t bytes; + + if (frame->size < 2) { + print_text(COLOR_ERROR, "invalid size"); + packet_hexdump(frame->data, frame->size); + return; + } + + bytes = bt_get_be16(frame->data); + + print_field("Attribute bytes: %d", bytes); + + packet_hexdump(frame->data + 2, bytes); + + print_continuation(frame->data + 2 + bytes, frame->size - 2 - bytes); +} + +static void search_attr_req(const struct l2cap_frame *frame) +{ + uint16_t search_bytes, attr_bytes; + + search_bytes = get_bytes(frame->data, frame->size); + + print_field("Search pattern: [len %d]", search_bytes); + packet_hexdump(frame->data, search_bytes); + + print_field("Max record count: %d", + bt_get_be16(frame->data + search_bytes)); + + attr_bytes = get_bytes(frame->data + search_bytes + 2, + frame->size - search_bytes - 2); + + print_field("Attribte list: [len %d]", attr_bytes); + packet_hexdump(frame->data + search_bytes + 2, attr_bytes); + + print_continuation(frame->data + search_bytes + 2 + attr_bytes, + frame->size - search_bytes - 2 - attr_bytes); +} + +static void search_attr_rsp(const struct l2cap_frame *frame) +{ + uint16_t bytes; + + if (frame->size < 2) { + print_text(COLOR_ERROR, "invalid size"); + packet_hexdump(frame->data, frame->size); + return; + } + + bytes = bt_get_be16(frame->data); + + print_field("Attribute list bytes: %d", bytes); + + packet_hexdump(frame->data + 2, bytes); + + print_continuation(frame->data + 2 + bytes, frame->size - 2 - bytes); +} + +struct sdp_data { + uint8_t pdu; + const char *str; + void (*func) (const struct l2cap_frame *frame); +}; + +static const struct sdp_data sdp_table[] = { + { 0x01, "Error Response", error_rsp }, + { 0x02, "Service Search Request", service_req }, + { 0x03, "Service Search Response", service_rsp }, + { 0x04, "Service Attribute Request", attr_req }, + { 0x05, "Service Attribute Response", attr_rsp }, + { 0x06, "Service Search Attribute Request", search_attr_req }, + { 0x07, "Service Search Attribute Response", search_attr_rsp }, + { } +}; + +void sdp_packet(const struct l2cap_frame *frame) +{ + uint8_t pdu; + uint16_t tid, plen; + struct l2cap_frame sdp_frame; + const struct sdp_data *sdp_data = NULL; + const char *pdu_color, *pdu_str; + + int i; + + if (frame->size < 5) { + print_text(COLOR_ERROR, "frame too short"); + packet_hexdump(frame->data, frame->size); + return; + } + + pdu = *((uint8_t *) frame->data); + tid = bt_get_be16(frame->data + 1); + plen = bt_get_be16(frame->data + 3); + + if (frame->size != plen + 5) { + print_text(COLOR_ERROR, "invalid frame size"); + packet_hexdump(frame->data, 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); + + if (!sdp_data || !sdp_data->func) { + packet_hexdump(frame->data + 5, frame->size - 5); + return; + } + + l2cap_frame_pull(&sdp_frame, frame, 5); + sdp_data->func(&sdp_frame); +} diff --git a/monitor/sdp.h b/monitor/sdp.h new file mode 100644 index 000000000..5287bb17c --- /dev/null +++ b/monitor/sdp.h @@ -0,0 +1,25 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +void sdp_packet(const struct l2cap_frame *frame); -- 2.47.3