[BlueZ,1/2] monitor/att: Add handler support for Read by Type

diff --git a/monitor/att.c b/monitor/att.c
index fbd75db03b83..289f4fc04d9a 100644
--- a/monitor/att.c
+++ b/monitor/att.c
@@ -43,6 +43,21 @@ 
 #include "l2cap.h"
 #include "att.h"
 
+struct att_read {
+	struct gatt_db_attribute *attr;
+	bool in;
+	uint16_t chan;
+	void (*func)(const struct l2cap_frame *frame);
+};
+
+struct att_conn_data {
+	struct gatt_db *ldb;
+	struct timespec ldb_mtim;
+	struct gatt_db *rdb;
+	struct timespec rdb_mtim;
+	struct queue *reads;
+};
+
 static void print_uuid(const char *label, const void *data, uint16_t size)
 {
 	const char *str;
@@ -77,27 +92,66 @@  static void print_handle_range(const char *label, const void *data)
 				get_le16(data), get_le16(data + 2));
 }
 
-static void print_data_list(const char *label, uint8_t length,
-					const void *data, uint16_t size)
+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;
+
+	/* Read frame and response frame shall be in the opposite direction to
+	 * match.
+	 */
+	if (read->in == frame->in)
+		return false;
+
+	return read->chan == frame->chan;
+}
+
+static void print_data_list(const char *label, uint8_t length,
+					const struct l2cap_frame *frame)
+{
+	struct packet_conn_data *conn;
+	struct att_conn_data *data;
+	struct att_read *read;
 	uint8_t count;
 
 	if (length == 0)
 		return;
 
-	count = size / length;
+	conn = packet_get_conn_data(frame->handle);
+	if (conn) {
+		data = conn->data;
+		if (data)
+			read = queue_remove_if(data->reads, match_read_frame,
+						(void *)frame);
+		else
+			read = NULL;
+	} else
+		read = NULL;
+
+	count = frame->size / length;
 
 	print_field("%s: %u entr%s", label, count, count == 1 ? "y" : "ies");
 
-	while (size >= length) {
-		print_field("Handle: 0x%4.4x", get_le16(data));
-		print_hex_field("Value", data + 2, length - 2);
+	while (frame->size >= length) {
+		if (!l2cap_frame_print_le16((void *)frame, "Handle"))
+			break;
 
-		data += length;
-		size -= length;
+		print_hex_field("Value", frame->data, length - 2);
+
+		if (read) {
+			struct l2cap_frame f;
+
+			l2cap_frame_clone_size(&f, frame, length - 2);
+
+			read->func(&f);
+		}
+
+		if (!l2cap_frame_pull((void *)frame, frame, length - 2))
+			break;
 	}
 
-	packet_hexdump(data, size);
+	packet_hexdump(frame->data, frame->size);
+	free(read);
 }
 
 static void print_attribute_info(uint16_t type, const void *data, uint16_t len)
@@ -2292,9 +2346,8 @@  struct gatt_handler {
 	GATT_HANDLER(0x2bba, content_control_id_read, NULL, NULL),
 };
 
-static struct gatt_handler *get_handler(struct gatt_db_attribute *attr)
+static struct gatt_handler *get_handler_uuid(const bt_uuid_t *uuid)
 {
-	const bt_uuid_t *uuid = gatt_db_attribute_get_type(attr);
 	size_t i;
 
 	for (i = 0; i < ARRAY_SIZE(gatt_handlers); i++) {
@@ -2307,6 +2360,11 @@  static struct gatt_handler *get_handler(struct gatt_db_attribute *attr)
 	return NULL;
 }
 
+static struct gatt_handler *get_handler(struct gatt_db_attribute *attr)
+{
+	return get_handler_uuid(gatt_db_attribute_get_type(attr));
+}
+
 static void att_exchange_mtu_req(const struct l2cap_frame *frame)
 {
 	const struct bt_l2cap_att_exchange_mtu_req *pdu = frame->data;
@@ -2403,36 +2461,23 @@  static void att_find_by_type_val_rsp(const struct l2cap_frame *frame)
 	packet_hexdump(ptr, len);
 }
 
-static void att_read_type_req(const struct l2cap_frame *frame)
+static int bt_uuid_from_data(bt_uuid_t *uuid, const void *data, uint16_t size)
 {
-	print_handle_range("Handle range", frame->data);
-	print_uuid("Attribute type", frame->data + 4, frame->size - 4);
+	uint128_t u128;
+
+	switch (size) {
+	case 2:
+		return bt_uuid16_create(uuid, get_le16(data));
+	case 4:
+		return bt_uuid32_create(uuid, get_le32(data));
+	case 16:
+		memcpy(u128.data, data, sizeof(u128.data));
+		return bt_uuid128_create(uuid, u128);
+	}
+
+	return -EINVAL;
 }
 
-static void att_read_type_rsp(const struct l2cap_frame *frame)
-{
-	const struct bt_l2cap_att_read_group_type_rsp *pdu = frame->data;
-
-	print_field("Attribute data length: %d", pdu->length);
-	print_data_list("Attribute data list", pdu->length,
-					frame->data + 1, frame->size - 1);
-}
-
-struct att_read {
-	struct gatt_db_attribute *attr;
-	bool in;
-	uint16_t chan;
-	void (*func)(const struct l2cap_frame *frame);
-};
-
-struct att_conn_data {
-	struct gatt_db *ldb;
-	struct timespec ldb_mtim;
-	struct gatt_db *rdb;
-	struct timespec rdb_mtim;
-	struct queue *reads;
-};
-
 static void att_conn_data_free(void *data)
 {
 	struct att_conn_data *att_data = data;
@@ -2443,6 +2488,67 @@  static void att_conn_data_free(void *data)
 	free(att_data);
 }
 
+static struct att_conn_data *att_get_conn_data(struct packet_conn_data *conn)
+{
+	struct att_conn_data *data = conn->data;
+
+	if (data)
+		return data;
+
+	data = new0(struct att_conn_data, 1);
+	data->rdb = gatt_db_new();
+	data->ldb = gatt_db_new();
+	conn->data = data;
+	conn->destroy = att_conn_data_free;
+
+	return data;
+}
+
+static void att_read_type_req(const struct l2cap_frame *frame)
+{
+	bt_uuid_t uuid;
+	struct packet_conn_data *conn;
+	struct att_conn_data *data;
+	struct att_read *read;
+	struct gatt_handler *handler;
+
+	print_handle_range("Handle range", frame->data);
+	print_uuid("Attribute type", frame->data + 4, frame->size - 4);
+
+	if (bt_uuid_from_data(&uuid, frame->data + 4, frame->size - 4))
+		return;
+
+	handler = get_handler_uuid(&uuid);
+	if (!handler || !handler->read)
+		return;
+
+	conn = packet_get_conn_data(frame->handle);
+	data = att_get_conn_data(conn);
+
+	if (!data->reads)
+		data->reads = queue_new();
+
+	read = new0(struct att_read, 1);
+	read->in = frame->in;
+	read->chan = frame->chan;
+	read->func = handler->read;
+
+	queue_push_tail(data->reads, read);
+}
+
+static void att_read_type_rsp(const struct l2cap_frame *frame)
+{
+	uint8_t len;
+
+	if (!l2cap_frame_get_u8((void *)frame, &len)) {
+		print_text(COLOR_ERROR, "invalid size");
+		return;
+	}
+
+	print_field("Attribute data length: %d", len);
+	print_data_list("Attribute data list", len, frame);
+}
+
 static void gatt_load_db(struct gatt_db *db, const char *filename,
 						struct timespec *mtim)
 {
@@ -2467,19 +2573,11 @@  static void gatt_load_db(struct gatt_db *db, const char *filename,
 
 static void load_gatt_db(struct packet_conn_data *conn)
 {
-	struct att_conn_data *data = conn->data;
+	struct att_conn_data *data = att_get_conn_data(conn);
 	char filename[PATH_MAX];
 	char local[18];
 	char peer[18];
 
-	if (!data) {
-		data = new0(struct att_conn_data, 1);
-		data->rdb = gatt_db_new();
-		data->ldb = gatt_db_new();
-		conn->data = data;
-		conn->destroy = att_conn_data_free;
-	}
-
 	ba2str((bdaddr_t *)conn->src, local);
 	ba2str((bdaddr_t *)conn->dst, peer);
 
@@ -2605,20 +2703,6 @@  static void att_read_req(const struct l2cap_frame *frame)
 	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;
-
-	/* Read frame and response frame shall be in the opposite direction to
-	 * match.
-	 */
-	if (read->in == frame->in)
-		return false;
-
-	return read->chan == frame->chan;
-}
-
 static void att_read_rsp(const struct l2cap_frame *frame)
 {
 	struct packet_conn_data *conn;
diff --git a/monitor/l2cap.h b/monitor/l2cap.h
index e63377857cf7..935066e6e7dc 100644
--- a/monitor/l2cap.h
+++ b/monitor/l2cap.h
@@ -31,8 +31,9 @@  void l2cap_frame_init(struct l2cap_frame *frame, uint16_t index, bool in,
 				uint16_t cid, uint16_t psm,
 				const void *data, uint16_t size);
 
-static inline void l2cap_frame_clone(struct l2cap_frame *frame,
-				const struct l2cap_frame *source)
+static inline void l2cap_frame_clone_size(struct l2cap_frame *frame,
+				const struct l2cap_frame *source,
+				uint16_t size)
 {
 	if (frame != source) {
 		frame->index   = source->index;
@@ -44,10 +45,16 @@  static inline void l2cap_frame_clone(struct l2cap_frame *frame,
 		frame->chan    = source->chan;
 		frame->mode    = source->mode;
 		frame->data    = source->data;
-		frame->size    = source->size;
+		frame->size    = size;
 	}
 }
 
+static inline void l2cap_frame_clone(struct l2cap_frame *frame,
+				const struct l2cap_frame *source)
+{
+	l2cap_frame_clone_size(frame, source, source->size);
+}
+
 static inline void *l2cap_frame_pull(struct l2cap_frame *frame,
 				const struct l2cap_frame *source, uint16_t len)
 {