From 4f0edb819069c7150768626decd75301ea7d8a0a Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Fri, 20 May 2022 15:51:42 -0700 Subject: [PATCH] monitor/att: Add decoding support for PAC Sink/Source This adds decoding support for PAC Sink/Source attributes: < ACL Data TX: Handle 42 flags 0x00 dlen 9 Channel: 64 len 5 sdu 3 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} ATT: Read Request (0x0a) len 2 Handle: 0x0017 Type: Sink PAC (0x2bc9) > ACL Data RX: Handle 42 flags 0x02 dlen 31 Channel: 65 len 27 sdu 25 [PSM 39 mode Enhanced Credit (0x81)] {chan 0} Value: 010600000000100301ff0002020302030305041e00f00000 Number of PAC(s): 1 PAC #0: Codec: LC3 (0x06) Codec Specific Configuration #0: len 0x03 type 0x01 Codec Specific Configuration: ff00 Codec Specific Configuration #1: len 0x02 type 0x02 Codec Specific Configuration: 03 Codec Specific Configuration #2: len 0x02 type 0x03 Codec Specific Configuration: 03 Codec Specific Configuration #3: len 0x05 type 0x04 Codec Specific Configuration: 1e00f000 Signed-off-by: Manika Shrivastava Signed-off-by: Ayush Garg --- monitor/att.c | 92 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ monitor/l2cap.h | 10 +++++- monitor/packet.c | 10 ++++++ monitor/packet.h | 2 ++ 4 files changed, 113 insertions(+), 1 deletion(-) diff --git a/monitor/att.c b/monitor/att.c index ad6bc04..a8cfa6c 100644 --- a/monitor/att.c +++ b/monitor/att.c @@ -246,6 +246,96 @@ static void ccc_write(const struct l2cap_frame *frame) print_ccc_value(frame); } +static void print_pac(const struct l2cap_frame *frame) +{ + uint8_t num = 0, i; + + if (!l2cap_frame_get_u8((void *)frame, &num)) { + print_text(COLOR_ERROR, "Number of PAC(s): invalid size"); + goto done; + } + + print_field(" Number of PAC(s): %u", num); + + for (i = 0; i < num; i++) { + uint8_t codec_id; + uint16_t codec_cid, codec_vid; + struct bt_hci_lv_data *cc; + struct bt_hci_lv_data *meta; + + print_field(" PAC #%u:", i); + + if (!l2cap_frame_get_u8((void *)frame, &codec_id)) { + print_text(COLOR_ERROR, "Codec: invalid size"); + goto done; + } + + packet_print_codec_id(" Codec", codec_id); + + if (!l2cap_frame_get_le16((void *)frame, &codec_cid)) { + print_text(COLOR_ERROR, + "Codec Company ID: invalid size"); + goto done; + } + + if (!l2cap_frame_get_le16((void *)frame, &codec_vid)) { + print_text(COLOR_ERROR, + "Codec Vendor ID: invalid size"); + goto done; + } + + if (codec_id == 0xff) { + print_field(" Codec Company ID: %s (0x%04x)", + bt_compidtostr(codec_cid), + codec_cid); + print_field(" Codec Vendor ID: 0x%04x", codec_vid); + } + + cc = l2cap_frame_pull((void *)frame, frame, sizeof(*cc)); + if (!cc) { + print_text(COLOR_ERROR, + "Codec Specific Configuration: invalid size"); + goto done; + } + + if (!l2cap_frame_pull((void *)frame, frame, cc->len)) { + print_text(COLOR_ERROR, + "Codec Specific Configuration: invalid size"); + goto done; + } + + packet_print_ltv(" Codec Specific Configuration", cc->data, + cc->len); + + meta = l2cap_frame_pull((void *)frame, frame, sizeof(*meta)); + if (!meta) { + print_text(COLOR_ERROR, "Metadata: invalid size"); + goto done; + } + + if (!l2cap_frame_pull((void *)frame, frame, meta->len)) { + print_text(COLOR_ERROR, "Metadata: invalid size"); + goto done; + } + + packet_print_ltv(" Metadata", meta->data, meta->len); + } + +done: + if (frame->size) + print_hex_field(" Data", frame->data, frame->size); +} + +static void pac_read(const struct l2cap_frame *frame) +{ + print_pac(frame); +} + +static void pac_notify(const struct l2cap_frame *frame) +{ + print_pac(frame); +} + #define GATT_HANDLER(_uuid, _read, _write, _notify) \ { \ .uuid = { \ @@ -264,6 +354,8 @@ struct gatt_handler { void (*notify)(const struct l2cap_frame *frame); } gatt_handlers[] = { GATT_HANDLER(0x2902, ccc_read, ccc_write, NULL), + GATT_HANDLER(0x2bc9, pac_read, NULL, pac_notify), + GATT_HANDLER(0x2bcb, pac_read, NULL, pac_notify), }; static struct gatt_handler *get_handler(struct gatt_db_attribute *attr) diff --git a/monitor/l2cap.h b/monitor/l2cap.h index 1daeb69..c33d4c5 100755 --- a/monitor/l2cap.h +++ b/monitor/l2cap.h @@ -48,13 +48,21 @@ static inline void l2cap_frame_clone(struct l2cap_frame *frame, } } -static inline void l2cap_frame_pull(struct l2cap_frame *frame, +static inline void *l2cap_frame_pull(struct l2cap_frame *frame, const struct l2cap_frame *source, uint16_t len) { + void *data; + l2cap_frame_clone(frame, source); + if (source->size < len) + return NULL; + + data = (void *)frame->data; frame->data = source->data + len; frame->size = source->size - len; + + return data; } static inline bool l2cap_frame_get_u8(struct l2cap_frame *frame, uint8_t *value) diff --git a/monitor/packet.c b/monitor/packet.c index 4fc659b..98dda9e 100755 --- a/monitor/packet.c +++ b/monitor/packet.c @@ -1376,6 +1376,11 @@ static void print_codec_id(const char *label, uint8_t codec) print_field("%s: %s (0x%2.2x)", label, str, codec); } +void packet_print_codec_id(const char *label, uint8_t codec) +{ + print_codec_id(label, codec); +} + static const struct bitfield_data codec_transport_table[] = { { 0, "Codec supported over BR/EDR ACL" }, { 1, "Codec supported over BR/EDR SCO and eSCO"}, @@ -3367,6 +3372,11 @@ static void print_ltv(const char *label, const uint8_t *data, uint8_t len) print_hex_field(label, iov.iov_base, iov.iov_len); } +void packet_print_ltv(const char *label, const uint8_t *data, uint8_t len) +{ + print_ltv(label, data, len); +} + static void print_base_annoucement(const uint8_t *data, uint8_t data_len) { struct iovec iov; diff --git a/monitor/packet.h b/monitor/packet.h index cc93063..f942974 100755 --- a/monitor/packet.h +++ b/monitor/packet.h @@ -61,6 +61,8 @@ void packet_print_channel_map_lmp(const uint8_t *map); void packet_print_channel_map_ll(const uint8_t *map); void packet_print_io_capability(uint8_t capability); void packet_print_io_authentication(uint8_t authentication); +void packet_print_codec_id(const char *label, uint8_t codec); +void packet_print_ltv(const char *label, const uint8_t *data, uint8_t len); void packet_control(struct timeval *tv, struct ucred *cred, uint16_t index, uint16_t opcode, -- 2.7.4