monitor/att: Add decoding support for CCC

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)

Signed-off-by: Manika Shrivastava <manika.sh@samsung.com>
Signed-off-by: Ayush Garg <ayush.garg@samsung.com>

diff --git a/monitor/att.c b/monitor/att.c
index 1c1e8d2..9323796 100644 (file)
--- a/monitor/att.c
+++ b/monitor/att.c
@@ -214,6 +214,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;
@@ -325,9 +401,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)
@@ -336,6 +419,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);
 }
 
@@ -440,13 +524,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;
+
+       l2cap_frame_pull((void *)frame, frame, sizeof(*pdu));
 
-       print_handle(frame, le16_to_cpu(pdu->handle), false);
+       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)
@@ -508,10 +646,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)
@@ -552,20 +721,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)
@@ -590,13 +788,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);
        }
@@ -604,14 +796,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);
 }