Diff between 53c28e3d868396fe5676751a44171dd67a7255a0 and 13bdb9f3bee1763143ec5af4db3bb4e213261fe7

Changed Files

File Additions Deletions Status
monitor/att.c +92 -0 modified
monitor/l2cap.h +9 -1 modified
monitor/packet.c +10 -0 modified
monitor/packet.h +2 -0 modified

Full Patch

diff --git a/monitor/att.c b/monitor/att.c
index 0223af2..de27ee4 100644
--- a/monitor/att.c
+++ b/monitor/att.c
@@ -247,6 +247,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 = { \
@@ -265,6 +355,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 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 e854c1a..c7739fb 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 a00975e..97d683e 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,