monitor: Decode vendor codecs of BT_HCI_CMD_READ_LOCAL_CODECS_V2

This attempts to decode the vendor codecs includec in the response of
BT_HCI_CMD_READ_LOCAL_CODECS_V2.

diff --git a/monitor/bt.h b/monitor/bt.h
index 0c6dcdf..e8dd0a1 100755 (executable)
--- a/monitor/bt.h
+++ b/monitor/bt.h
@@ -1787,6 +1787,12 @@ struct bt_hci_rsp_read_local_pairing_options {
 #define BT_HCI_LOCAL_CODEC_LE_CIS              BIT(2)
 #define BT_HCI_LOCAL_CODEC_LE_BIS              BIT(3)
 
+struct bt_hci_vnd_codec_v2 {
+       uint16_t cid;
+       uint16_t vid;
+       uint8_t  transport;
+} __attribute__ ((packed));
+
 struct bt_hci_vnd_codec {
        uint8_t  id;
        uint16_t cid;

diff --git a/monitor/packet.c b/monitor/packet.c
index 84488af..f9286c7 100755 (executable)
--- a/monitor/packet.c
+++ b/monitor/packet.c
@@ -6378,6 +6378,20 @@ static void print_list(const void *data, uint8_t size, int num_items,
                print_hex_field("", data, size);
 }
 
+static void print_vnd_codecs_v2(const void *data, int i)
+{
+       const struct bt_hci_vnd_codec_v2 *codec = data;
+       uint8_t mask;
+
+       packet_print_company("  Company ID", le16_to_cpu(codec->cid));
+       print_field("  Vendor Codec ID: 0x%4.4x", le16_to_cpu(codec->vid));
+       print_field("  Logical Transport Type: 0x%02x", codec->transport);
+       mask = print_bitfield(4, codec->transport, codec_transport_table);
+       if (mask)
+               print_text(COLOR_UNKNOWN_SERVICE_CLASS,
+                               "  Unknown transport (0x%2.2x)", mask);
+}
+
 static void read_local_codecs_rsp_v2(uint16_t index, const void *data,
                                                        uint8_t size)
 {
@@ -6411,7 +6425,18 @@ static void read_local_codecs_rsp_v2(uint16_t index, const void *data,
 
        num_vnd_codecs = rsp->codec[rsp->num_codecs].id;
 
+       size -= 1;
+
        print_field("Number of vendor codecs: %d", num_vnd_codecs);
+
+       if (size < num_vnd_codecs * sizeof(*rsp->codec)) {
+               print_field("Invalid number of vendor codecs.");
+               return;
+       }
+
+       print_list(&rsp->codec[rsp->num_codecs] + 1, size, num_vnd_codecs,
+                       sizeof(struct bt_hci_vnd_codec_v2),
+                       print_vnd_codecs_v2);
 }
 
 static void print_path_direction(const char *prefix, uint8_t dir)