From b04ef47eadac768c8c94a3dd5f85f657a17b4e6d Mon Sep 17 00:00:00 2001 From: Luiz Augusto von Dentz Date: Wed, 24 Feb 2021 16:50:01 -0800 Subject: [PATCH] monitor: Add support for decoding Read Local Supported Codecs V2 < HCI Command: Read Local Supported Codecs V2 (0x04|0x000d) plen 0 > HCI Event: Command Complete (0x0e) plen 18 Read Local Supported Codecs V2 (0x04|0x000d) ncmd 1 Status: Success (0x00) Number of supported codecs: 6 Codec: u-law log (0x00) Logical Transport Type: 0x02 Codec supported over BR/EDR SCO and eSCO Codec: A-law log (0x01) Logical Transport Type: 0x02 Codec supported over BR/EDR SCO and eSCO Codec: CVSD (0x02) Logical Transport Type: 0x02 Codec supported over BR/EDR SCO and eSCO Codec: Transparent (0x03) Logical Transport Type: 0x02 Codec supported over BR/EDR SCO and eSCO Codec: Linear PCM (0x04) Logical Transport Type: 0x02 Codec supported over BR/EDR SCO and eSCO Codec: mSBC (0x05) Logical Transport Type: 0x02 Codec supported over BR/EDR SCO and eSCO Number of vendor codecs: 0 --- monitor/bt.h | 24 +++++++++ monitor/packet.c | 124 +++++++++++++++++++++++++++++++++++++---------- 2 files changed, 122 insertions(+), 26 deletions(-) diff --git a/monitor/bt.h b/monitor/bt.h index 39110b0b3..afb0deaf5 100644 --- a/monitor/bt.h +++ b/monitor/bt.h @@ -1768,6 +1768,30 @@ struct bt_hci_rsp_read_local_pairing_options { uint8_t max_key_size; } __attribute__ ((packed)); +#define BT_HCI_CMD_READ_LOCAL_CODECS_V2 0x100d +#define BT_HCI_BIT_READ_LOCAL_CODECS_V2 BT_HCI_CMD_BIT(45, 2) +#define BT_HCI_LOCAL_CODEC_BREDR_ACL BIT(0) +#define BT_HCI_LOCAL_CODEC_BREDR_SCO BIT(1) +#define BT_HCI_LOCAL_CODEC_LE_CIS BIT(2) +#define BT_HCI_LOCAL_CODEC_LE_BIS BIT(3) + +struct bt_hci_vnd_codec { + uint16_t company; + uint16_t id; + uint8_t transport; +} __attribute__ ((packed)); + +struct bt_hci_codec { + uint8_t id; + uint8_t transport; +} __attribute__ ((packed)); + +struct bt_hci_rsp_read_local_codecs_v2 { + uint8_t status; + uint8_t num_codecs; + struct bt_hci_codec codec[0]; +} __attribute__ ((packed)); + #define BT_HCI_CMD_READ_FAILED_CONTACT_COUNTER 0x1401 struct bt_hci_cmd_read_failed_contact_counter { uint16_t handle; diff --git a/monitor/packet.c b/monitor/packet.c index 87cb0f756..bacbe2c53 100644 --- a/monitor/packet.c +++ b/monitor/packet.c @@ -1305,7 +1305,7 @@ static void print_air_mode(uint8_t mode) print_field("Air mode: %s (0x%2.2x)", str, mode); } -static void print_codec(const char *label, uint8_t codec) +static void print_codec_id(const char *label, uint8_t codec) { const char *str; @@ -1339,6 +1339,26 @@ static void print_codec(const char *label, uint8_t codec) print_field("%s: %s (0x%2.2x)", label, str, 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"}, + { 2, "Codec supported over LE CIS" }, + { 3, "Codec supported over LE BIS" }, + { } +}; + +static void print_codec(const char *label, const struct bt_hci_codec *codec) +{ + uint8_t mask; + + print_codec_id(label, codec->id); + 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 print_inquiry_mode(uint8_t mode) { const char *str; @@ -5886,7 +5906,7 @@ static void read_local_codecs_rsp(const void *data, uint8_t size) print_field("Number of supported codecs: %d", rsp->num_codecs); for (i = 0; i < rsp->num_codecs; i++) - print_codec(" Codec", rsp->codec[i]); + print_codec_id(" Codec", rsp->codec[i]); num_vnd_codecs = rsp->codec[rsp->num_codecs]; @@ -5896,6 +5916,67 @@ static void read_local_codecs_rsp(const void *data, uint8_t size) size - rsp->num_codecs - 3); } +static void print_codecs(const void *data, int i) +{ + const struct bt_hci_codec *codec = data; + + print_codec(" Codec", codec); +} + +typedef void (*print_list_func_t)(const void *data, int i); + +static void print_list(const void *data, uint8_t size, int num_items, + size_t item_size, print_list_func_t func) +{ + int i; + + for (i = 0; size >= item_size && num_items; i++) { + if (func) + func(data, i); + data += item_size; + size -= item_size; + num_items--; + } + + if (num_items) + print_hex_field("", data, size); +} + +static void read_local_codecs_rsp_v2(const void *data, uint8_t size) +{ + const struct bt_hci_rsp_read_local_codecs_v2 *rsp = data; + uint8_t num_vnd_codecs; + + if (rsp->num_codecs + 3 > size) { + print_field("Invalid number of codecs."); + return; + } + + print_status(rsp->status); + print_field("Number of supported codecs: %d", rsp->num_codecs); + + size -= sizeof(*rsp); + + if (size < rsp->num_codecs * sizeof(*rsp->codec)) { + print_field("Invalid number of codecs."); + return; + } + + print_list(rsp->codec, size, rsp->num_codecs, sizeof(*rsp->codec), + print_codecs); + + size -= rsp->num_codecs * sizeof(*rsp->codec); + + if (size < sizeof(uint8_t)) { + print_field("Invalid number of vendor codecs."); + return; + } + + num_vnd_codecs = rsp->codec[rsp->num_codecs].id; + + print_field("Number of vendor codecs: %d", num_vnd_codecs); +} + static void read_local_pairing_options_rsp(const void *data, uint8_t size) { const struct bt_hci_rsp_read_local_pairing_options *rsp = data; @@ -7795,7 +7876,7 @@ static void le_read_iso_tx_sync_rsp(const void *data, uint8_t size) print_field("Offset: %d", le32_to_cpu(offset)); } -static void print_cis_params(const void *data) +static void print_cis_params(const void *data, int i) { const struct bt_hci_cis_params *cis = data; @@ -7812,20 +7893,6 @@ static void print_cis_params(const void *data) cis->s_rtn); } -static void print_list(const void *data, uint8_t size, int num_items, - size_t item_size, void (*callback)(const void *data)) -{ - while (size >= item_size && num_items) { - callback(data); - data += item_size; - size -= item_size; - num_items--; - } - - if (num_items) - print_hex_field("", data, size); -} - static void print_usec_interval(const char *prefix, const uint8_t interval[3]) { uint32_t u24 = 0; @@ -7857,7 +7924,7 @@ static void le_set_cig_params_cmd(const void *data, uint8_t size) print_cis_params); } -static void print_cis_params_test(const void *data) +static void print_cis_params_test(const void *data, int i) { const struct bt_hci_cis_params_test *cis = data; @@ -7898,7 +7965,7 @@ static void le_set_cig_params_test_cmd(const void *data, uint8_t size) print_cis_params_test); } -static void print_cig_handle(const void *data) +static void print_cig_handle(const void *data, int i) { const uint16_t *handle = data; @@ -7923,7 +7990,7 @@ static void le_set_cig_params_rsp(const void *data, uint8_t size) print_cig_handle); } -static void print_cis(const void *data) +static void print_cis(const void *data, int i) { const struct bt_hci_cis *cis = data; @@ -7976,7 +8043,7 @@ static void le_reject_cis_req_cmd(const void *data, uint8_t size) print_reason(cmd->reason); } -static void print_bis(const void *data) +static void print_bis(const void *data, int i) { const struct bt_hci_bis *bis = data; @@ -8005,7 +8072,7 @@ static void le_create_big_cmd(const void *data, uint8_t size) print_list(cmd->bis, size, cmd->num_bis, sizeof(*cmd->bis), print_bis); } -static void print_bis_test(const void *data) +static void print_bis_test(const void *data, int i) { const struct bt_hci_bis_test *bis = data; @@ -8048,7 +8115,7 @@ static void le_terminate_big_cmd(const void *data, uint8_t size) print_reason(cmd->reason); } -static void print_bis_sync(const void *data) +static void print_bis_sync(const void *data, int i) { const uint8_t *bis_id = data; @@ -8120,7 +8187,7 @@ static void le_setup_iso_path_cmd(const void *data, uint8_t size) print_field("Handle: %d", le16_to_cpu(cmd->handle)); print_iso_dir("Data Path Direction", cmd->direction); print_iso_path("Data Path", cmd->path); - print_codec("Coding Format", cmd->codec); + print_codec_id("Coding Format", cmd->codec); packet_print_company("Company Codec ID", le16_to_cpu(cmd->codec_cid)); print_field("Vendor Codec ID: %d", le16_to_cpu(cmd->codec_vid)); print_usec_interval("Controller Delay", cmd->delay); @@ -8680,7 +8747,12 @@ static const struct opcode_data opcode_table[] = { { 0x100c, 331, "Read Local Simple Pairing Options", null_cmd, 0, true, read_local_pairing_options_rsp, 3, true }, - + { BT_HCI_CMD_READ_LOCAL_CODECS_V2, BT_HCI_BIT_READ_LOCAL_CODECS_V2, + "Read Local Supported Codecs V2", + null_cmd, 0, true, + read_local_codecs_rsp_v2, + sizeof(struct bt_hci_rsp_read_local_codecs_v2), false + }, /* OGF 5 - Status Parameter */ { 0x1401, 122, "Read Failed Contact Counter", read_failed_contact_counter_cmd, 2, true, @@ -10562,7 +10634,7 @@ static void le_req_cis_evt(const void *data, uint8_t size) print_field("CIS ID: 0x%2.2x", evt->cis_id); } -static void print_bis_handle(const void *data) +static void print_bis_handle(const void *data, int i) { const uint16_t *handle = data; -- 2.47.3