[v2,7/7] monitor/att: Add decoding support for CCC

Commit Message

Luiz Augusto von Dentz May 20, 2022, 4:17 a.m. UTC

From: Luiz Augusto von Dentz <[e-mail]>

This adds decoding support for CCC so its value can be decoded:

< ACL Data TX: Handle 3585 flags 0x00 dlen 7
      ATT: Read Request (0x0a) len 2
        Handle: 0x002c Type: Client Characteristic Configuration (0x2902)
> ACL Data RX: Handle 3585 flags 0x02 dlen 6
      ATT: Read Response (0x0b) len 1
        Value: 01
            Notification (0x01)
< ACL Data TX: Handle 3585 flags 0x00 dlen 9
      ATT: Write Request (0x12) len 4
        Handle: 0x002c Type: Client Characteristic Configuration (0x2902)
          Data: 0100
            Notification (0x01)
---
 monitor/att.c | 241 ++++++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 223 insertions(+), 18 deletions(-)

Patch

diff --git a/monitor/att.c b/monitor/att.c
index 304c37319..f1af420ba 100644
--- a/monitor/att.c
+++ b/monitor/att.c
@@ -215,6 +215,82 @@  static void att_error_response(const struct l2cap_frame *frame)
 	print_field("Error: %s (0x%2.2x)", str, pdu->error);
 }
 
+static const struct bitfield_data ccc_value_table[] = {
+	{  0, "Notification (0x01)"		},
+	{  1, "Indication (0x02)"		},
+	{ }
+};
+
+static void print_ccc_value(uint8_t value)
+{
+	uint8_t mask = value;
+
+	mask = print_bitfield(4, value, ccc_value_table);
+	if (mask)
+		print_text(COLOR_WHITE_BG, "    Unknown fields (0x%2.2x)",
+								mask);
+}
+
+static void gatt_ccc_read(const struct l2cap_frame *frame)
+{
+	uint8_t value;
+
+	if (!l2cap_frame_get_u8((void *)frame, &value)) {
+		print_text(COLOR_ERROR, "invalid size");
+		return;
+	}
+
+	print_ccc_value(value);
+}
+
+static void gatt_ccc_write(const struct l2cap_frame *frame)
+{
+	uint8_t value;
+
+	if (!l2cap_frame_get_u8((void *)frame, &value)) {
+		print_text(COLOR_ERROR, "invalid size");
+		return;
+	}
+
+	print_ccc_value(value);
+}
+
+#define GATT_HANDLER(_uuid, _read, _write, _notify) \
+{ \
+	.uuid = { \
+		.type = BT_UUID16, \
+		.value.u16 = _uuid, \
+	}, \
+	.read = _read, \
+	.write = _write, \
+	.notify = _notify \
+}
+
+struct gatt_handler {
+	bt_uuid_t uuid;
+	void (*read)(const struct l2cap_frame *frame);
+	void (*write)(const struct l2cap_frame *frame);
+	void (*notify)(const struct l2cap_frame *frame);
+} gatt_handlers[] = {
+	GATT_HANDLER(GATT_CLIENT_CHARAC_CFG_UUID, gatt_ccc_read,
+					gatt_ccc_write, NULL)
+};
+
+static struct gatt_handler *get_handler(struct gatt_db_attribute *attr)
+{
+	const bt_uuid_t *uuid = gatt_db_attribute_get_type(attr);
+	size_t i;
+
+	for (i = 0; i < ARRAY_SIZE(gatt_handlers); i++) {
+		struct gatt_handler *handler = &gatt_handlers[i];
+
+		if (!bt_uuid_cmp(&handler->uuid, uuid))
+			return handler;
+	}
+
+	return NULL;
+}
+
 static void att_exchange_mtu_req(const struct l2cap_frame *frame)
 {
 	const struct bt_l2cap_att_exchange_mtu_req *pdu = frame->data;
@@ -326,9 +402,16 @@  static void att_read_type_rsp(const struct l2cap_frame *frame)
 					frame->data + 1, frame->size - 1);
 }
 
+struct att_read {
+	struct gatt_db_attribute *attr;
+	uint16_t cid;
+	void (*func)(const struct l2cap_frame *frame);
+};
+
 struct att_conn_data {
 	struct gatt_db *ldb;
 	struct gatt_db *rdb;
+	struct queue *reads;
 };
 
 static void att_conn_data_free(void *data)
@@ -337,6 +420,7 @@  static void att_conn_data_free(void *data)
 
 	gatt_db_unref(att_data->rdb);
 	gatt_db_unref(att_data->ldb);
+	queue_destroy(att_data->reads, free);
 	free(att_data);
 }
 
@@ -441,13 +525,67 @@  done:
 static void att_read_req(const struct l2cap_frame *frame)
 {
 	const struct bt_l2cap_att_read_req *pdu = frame->data;
+	uint16_t handle;
+	struct packet_conn_data *conn;
+	struct att_conn_data *data;
+	struct att_read *read;
+	struct gatt_db_attribute *attr;
+	struct gatt_handler *handler;
 
-	print_handle(frame, le16_to_cpu(pdu->handle), false);
+	l2cap_frame_pull((void *)frame, frame, sizeof(*pdu));
+
+	handle = le16_to_cpu(pdu->handle);
+	print_handle(frame, handle, false);
+
+	attr = get_attribute(frame, handle, false);
+	if (!attr)
+		return;
+
+	handler = get_handler(attr);
+	if (!handler)
+		return;
+
+	conn = packet_get_conn_data(frame->handle);
+	data = conn->data;
+
+	if (!data->reads)
+		data->reads = queue_new();
+
+	read = new0(struct att_read, 1);
+	read->attr = attr;
+	read->cid = frame->cid;
+	read->func = handler->read;
+
+	queue_push_tail(data->reads, read);
+}
+
+static bool match_read_frame(const void *data, const void *match_data)
+{
+	const struct att_read *read = data;
+	const struct l2cap_frame *frame = match_data;
+
+	return read->cid == frame->cid;
 }
 
 static void att_read_rsp(const struct l2cap_frame *frame)
 {
+	struct packet_conn_data *conn;
+	struct att_conn_data *data;
+	struct att_read *read;
+
 	print_hex_field("Value", frame->data, frame->size);
+
+	conn = packet_get_conn_data(frame->handle);
+	if (!conn)
+		return;
+
+	data = conn->data;
+
+	read = queue_find(data->reads, match_read_frame, frame);
+	if (!read)
+		return;
+
+	read->func(frame);
 }
 
 static void att_read_blob_req(const struct l2cap_frame *frame)
@@ -509,10 +647,41 @@  static void att_read_group_type_rsp(const struct l2cap_frame *frame)
 					frame->data + 1, frame->size - 1);
 }
 
+static void print_write(const struct l2cap_frame *frame, uint16_t handle,
+							size_t len)
+{
+	struct gatt_db_attribute *attr;
+	struct gatt_handler *handler;
+
+	print_handle(frame, handle, false);
+	print_hex_field("  Data", frame->data, frame->size);
+
+	if (len > frame->size) {
+		print_text(COLOR_ERROR, "invalid size");
+		return;
+	}
+
+	attr = get_attribute(frame, handle, false);
+	if (!attr)
+		return;
+
+	handler = get_handler(attr);
+	if (!handler)
+		return;
+
+	handler->write(frame);
+}
+
 static void att_write_req(const struct l2cap_frame *frame)
 {
-	print_handle(frame, get_le16(frame->data), false);
-	print_hex_field("  Data", frame->data + 2, frame->size - 2);
+	uint16_t handle;
+
+	if (!l2cap_frame_get_le16((void *)frame, &handle)) {
+		print_text(COLOR_ERROR, "invalid size");
+		return;
+	}
+
+	print_write(frame, handle, frame->size);
 }
 
 static void att_write_rsp(const struct l2cap_frame *frame)
@@ -553,20 +722,49 @@  static void att_execute_write_req(const struct l2cap_frame *frame)
 	print_field("Flags: %s (0x%02x)", flags_str, flags);
 }
 
+static void print_notify(const struct l2cap_frame *frame, uint16_t handle,
+								size_t len)
+{
+	struct gatt_db_attribute *attr;
+	struct gatt_handler *handler;
+
+	print_handle(frame, handle, true);
+	print_hex_field("  Data", frame->data, len);
+
+	if (len > frame->size) {
+		print_text(COLOR_ERROR, "invalid size");
+		return;
+	}
+
+	attr = get_attribute(frame, handle, true);
+	if (!attr)
+		return;
+
+	handler = get_handler(attr);
+	if (!handler)
+		return;
+
+	handler->notify(frame);
+}
+
 static void att_handle_value_notify(const struct l2cap_frame *frame)
 {
+	uint16_t handle;
 	const struct bt_l2cap_att_handle_value_notify *pdu = frame->data;
 
-	print_handle(frame, le16_to_cpu(pdu->handle), true);
-	print_hex_field("  Data", frame->data + 2, frame->size - 2);
+	l2cap_frame_pull((void *)frame, frame, sizeof(*pdu));
+
+	handle = le16_to_cpu(pdu->handle);
+	print_notify(frame, handle, frame->size);
 }
 
 static void att_handle_value_ind(const struct l2cap_frame *frame)
 {
 	const struct bt_l2cap_att_handle_value_ind *pdu = frame->data;
 
-	print_handle(frame, le16_to_cpu(pdu->handle), true);
-	print_hex_field("  Data", frame->data + 2, frame->size - 2);
+	l2cap_frame_pull((void *)frame, frame, sizeof(*pdu));
+
+	print_notify(frame, le16_to_cpu(pdu->handle), frame->size);
 }
 
 static void att_handle_value_conf(const struct l2cap_frame *frame)
@@ -591,13 +789,7 @@  static void att_multiple_vl_rsp(const struct l2cap_frame *frame)
 
 		print_field("Length: 0x%4.4x", len);
 
-		print_hex_field("  Data", f->data,
-				len < f->size ? len : f->size);
-
-		if (len > f->size) {
-			print_text(COLOR_ERROR, "invalid size");
-			return;
-		}
+		print_notify(frame, handle, len);
 
 		l2cap_frame_pull(f, f, len);
 	}
@@ -605,14 +797,27 @@  static void att_multiple_vl_rsp(const struct l2cap_frame *frame)
 
 static void att_write_command(const struct l2cap_frame *frame)
 {
-	print_handle(frame, get_le16(frame->data), false);
-	print_hex_field("  Data", frame->data + 2, frame->size - 2);
+	uint16_t handle;
+
+	if (!l2cap_frame_get_le16((void *)frame, &handle)) {
+		print_text(COLOR_ERROR, "invalid size");
+		return;
+	}
+
+	print_write(frame, handle, frame->size);
 }
 
 static void att_signed_write_command(const struct l2cap_frame *frame)
 {
-	print_handle(frame, get_le16(frame->data), false);
-	print_hex_field("  Data", frame->data + 2, frame->size - 2 - 12);
+	uint16_t handle;
+
+	if (!l2cap_frame_get_le16((void *)frame, &handle)) {
+		print_text(COLOR_ERROR, "invalid size");
+		return;
+	}
+
+	print_write(frame, handle, frame->size - 12);
+	print_hex_field("  Data", frame->data, frame->size - 12);
 	print_hex_field("  Signature", frame->data + frame->size - 12, 12);
 }