From 681dfdc12bf6b628139a31b526b67e3337605933 Mon Sep 17 00:00:00 2001 From: Joakim Andersson Date: Wed, 4 Mar 2020 18:12:32 +0100 Subject: [PATCH] Bluetooth: HCI_VS: Check for supported command According to the Zephyr VS HCI specification: Only Read_Version_Information and Read_Supported_Commands commands are mandatory. Check for supported Read Supported Features command before issuing this command to the controller. Signed-off-by: Joakim Andersson --- include/bluetooth/hci_vs.h | 23 ++++++++++++++++++++ subsys/bluetooth/host/hci_core.c | 36 ++++++++++++++++++-------------- 2 files changed, 43 insertions(+), 16 deletions(-) diff --git a/include/bluetooth/hci_vs.h b/include/bluetooth/hci_vs.h index b8c36deac02..d3eb8844a82 100644 --- a/include/bluetooth/hci_vs.h +++ b/include/bluetooth/hci_vs.h @@ -15,6 +15,29 @@ extern "C" { #endif +#define BT_VS_CMD_BIT_VERSION 0 +#define BT_VS_CMD_BIT_SUP_CMD 1 +#define BT_VS_CMD_BIT_SUP_FEAT 2 +#define BT_VS_CMD_BIT_SET_EVT_MASK 3 +#define BT_VS_CMD_BIT_RESET 4 +#define BT_VS_CMD_BIT_WRITE_BDADDR 5 +#define BT_VS_CMD_BIT_SET_TRACE_ENABLE 6 +#define BT_VS_CMD_BIT_READ_BUILD_INFO 7 +#define BT_VS_CMD_BIT_READ_STATIC_ADDRS 8 +#define BT_VS_CMD_BIT_READ_KEY_ROOTS 9 +#define BT_VS_CMD_BIT_READ_CHIP_TEMP 10 +#define BT_VS_CMD_BIT_READ_HOST_STACK_CMD 11 +#define BT_VS_CMD_BIT_SET_SCAN_REP_ENABLE 12 +#define BT_VS_CMD_BIT_WRITE_TX_POWER 13 +#define BT_VS_CMD_BIT_READ_TX_POWER 14 + +#define BT_VS_CMD_SUP_FEAT(cmd) BT_LE_FEAT_TEST(cmd, \ + BT_VS_CMD_BIT_SUP_FEAT) +#define BT_VS_CMD_READ_STATIC_ADDRS(cmd) BT_LE_FEAT_TEST(cmd, \ + BT_VS_CMD_BIT_READ_STATIC_ADDRS) +#define BT_VS_CMD_READ_KEY_ROOTS(cmd) BT_LE_FEAT_TEST(cmd, \ + BT_VS_CMD_BIT_READ_KEY_ROOTS) + #define BT_HCI_VS_HW_PLAT_INTEL 0x0001 #define BT_HCI_VS_HW_PLAT_NORDIC 0x0002 #define BT_HCI_VS_HW_PLAT_NXP 0x0003 diff --git a/subsys/bluetooth/host/hci_core.c b/subsys/bluetooth/host/hci_core.c index b238fe42312..50ce549d904 100644 --- a/subsys/bluetooth/host/hci_core.c +++ b/subsys/bluetooth/host/hci_core.c @@ -5046,23 +5046,27 @@ static void hci_vs_init(void) memcpy(bt_dev.vs_commands, rp.cmds->commands, BT_DEV_VS_CMDS_MAX); net_buf_unref(rsp); - err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_SUPPORTED_FEATURES, - NULL, &rsp); - if (err) { - BT_WARN("Failed to read supported vendor features"); - return; - } + if (BT_VS_CMD_SUP_FEAT(bt_dev.vs_commands)) { + err = bt_hci_cmd_send_sync(BT_HCI_OP_VS_READ_SUPPORTED_FEATURES, + NULL, &rsp); + if (err) { + BT_WARN("Failed to read supported vendor features"); + return; + } - if (IS_ENABLED(CONFIG_BT_HCI_VS_EXT_DETECT) && - rsp->len != sizeof(struct bt_hci_rp_vs_read_supported_features)) { - BT_WARN("Invalid Vendor HCI extensions"); + if (IS_ENABLED(CONFIG_BT_HCI_VS_EXT_DETECT) && + rsp->len != + sizeof(struct bt_hci_rp_vs_read_supported_features)) { + BT_WARN("Invalid Vendor HCI extensions"); + net_buf_unref(rsp); + return; + } + + rp.feat = (void *)rsp->data; + memcpy(bt_dev.vs_features, rp.feat->features, + BT_DEV_VS_FEAT_MAX); net_buf_unref(rsp); - return; } - - rp.feat = (void *)rsp->data; - memcpy(bt_dev.vs_features, rp.feat->features, BT_DEV_VS_FEAT_MAX); - net_buf_unref(rsp); } #endif /* CONFIG_BT_HCI_VS_EXT */ @@ -5677,7 +5681,7 @@ static void bt_read_identity_root(u8_t *ir) struct net_buf *rsp; int err; - if (!(bt_dev.vs_commands[1] & BIT(1))) { + if (!BT_VS_CMD_READ_KEY_ROOTS(bt_dev.vs_commands)) { return; } @@ -5738,7 +5742,7 @@ static uint8_t bt_read_static_addr(struct bt_hci_vs_static_addr *addrs) int err, i; u8_t cnt; - if (!(bt_dev.vs_commands[1] & BIT(0))) { + if (!BT_VS_CMD_READ_STATIC_ADDRS(bt_dev.vs_commands)) { BT_WARN("Read Static Addresses command not available"); return 0; }