Received-SPF: pass (google.com: domain of linux-bluetooth-owner@vger.kernel.org designates 2620:137:e000::1:18 as permitted sender) client-ip=2620:137:e000::1:18;
From:   Luiz Augusto von Dentz <luiz.dentz@gmail.com>
To:     linux-bluetooth@vger.kernel.org
Subject: [PATCH BlueZ 1/3] monitor/att: Add decoding support for PAC Sink/Source
Date:   Wed, 25 May 2022 17:39:25 -0700
Message-Id: <20220526003927.3972965-1-luiz.dentz@gmail.com>
MIME-Version: 1.0
Content-Transfer-Encoding: 8bit
Precedence: bulk

From: Luiz Augusto von Dentz <luiz.von.dentz@intel.com>

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
---
 monitor/att.c    | 94 +++++++++++++++++++++++++++++++++++++++++++++++-
 monitor/l2cap.h  | 10 +++++-
 monitor/packet.c | 10 ++++++
 monitor/packet.h |  2 ++
 4 files changed, 114 insertions(+), 2 deletions(-)

diff --git a/monitor/att.c b/monitor/att.c
index df3e65057..2b1e21d05 100644
--- a/monitor/att.c
+++ b/monitor/att.c
@@ -255,6 +255,96 @@ static void gatt_ccc_write(const struct l2cap_frame *frame)
 	print_ccc_value(value);
 }
 
+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 = { \
@@ -273,7 +363,9 @@ struct gatt_handler {
 	void (*notify)(const struct l2cap_frame *frame);
 } gatt_handlers[] = {
 	GATT_HANDLER(GATT_CLIENT_CHARAC_CFG_UUID, gatt_ccc_read,
-					gatt_ccc_write, NULL)
+					gatt_ccc_write, NULL),
+	GATT_HANDLER(PAC_SINK_CHRC_UUID, pac_read, NULL, pac_notify),
+	GATT_HANDLER(PAC_SOURCE_CHRC_UUID, 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 1daeb69be..c33d4c57f 100644
--- 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 e854c1a8e..c7739fba5 100644
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -1377,6 +1377,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"},
@@ -3368,6 +3373,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 a00975eb3..97d683e3a 100644
--- 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.35.1