diff --git a/nimble/host/src/ble_hs_hci.c b/nimble/host/src/ble_hs_hci.c index c5b85a657..0996cfbb0 100644 --- a/nimble/host/src/ble_hs_hci.c +++ b/nimble/host/src/ble_hs_hci.c @@ -44,6 +44,8 @@ static uint32_t ble_hs_hci_sup_feat; static uint8_t ble_hs_hci_version; +static struct ble_hs_hci_sup_cmd ble_hs_hci_sup_cmd; + #if CONFIG_BT_NIMBLE_LEGACY_VHCI_ENABLE #define BLE_HS_HCI_FRAG_DATABUF_SIZE \ (BLE_ACL_MAX_PKT_SIZE + \ @@ -690,6 +692,18 @@ ble_hs_hci_get_hci_version(void) return ble_hs_hci_version; } +void +ble_hs_hci_set_hci_supported_cmd(struct ble_hs_hci_sup_cmd sup_cmd) +{ + ble_hs_hci_sup_cmd = sup_cmd; +} + +struct ble_hs_hci_sup_cmd +ble_hs_hci_get_hci_supported_cmd(void) +{ + return ble_hs_hci_sup_cmd; +} + void ble_hs_hci_init(void) { diff --git a/nimble/host/src/ble_hs_hci_priv.h b/nimble/host/src/ble_hs_hci_priv.h index ab862dc54..167c598b3 100644 --- a/nimble/host/src/ble_hs_hci_priv.h +++ b/nimble/host/src/ble_hs_hci_priv.h @@ -79,6 +79,10 @@ struct hci_periodic_adv_params #endif #endif +struct ble_hs_hci_sup_cmd { + unsigned event_mask2 : 1; /** Indicates whether the controller supports the set event mask page 2 command */ +}; + extern uint16_t ble_hs_hci_avail_pkts; /* This function is not waiting for command status/complete HCI events */ @@ -92,6 +96,8 @@ void ble_hs_hci_set_le_supported_feat(uint32_t feat); uint32_t ble_hs_hci_get_le_supported_feat(void); void ble_hs_hci_set_hci_version(uint8_t hci_version); uint8_t ble_hs_hci_get_hci_version(void); +void ble_hs_hci_set_hci_supported_cmd(struct ble_hs_hci_sup_cmd sup_cmd); +struct ble_hs_hci_sup_cmd ble_hs_hci_get_hci_supported_cmd(void); #if MYNEWT_VAL(BLE_HS_PHONY_HCI_ACKS) typedef int ble_hs_hci_phony_ack_fn(uint8_t *ack, int ack_buf_len); diff --git a/nimble/host/src/ble_hs_startup.c b/nimble/host/src/ble_hs_startup.c index 5d4b8fc48..96c75251b 100644 --- a/nimble/host/src/ble_hs_startup.c +++ b/nimble/host/src/ble_hs_startup.c @@ -68,6 +68,27 @@ ble_hs_startup_read_local_ver_tx(void) return 0; } +static int +ble_hs_startup_read_sup_cmd_tx(void) +{ + struct ble_hci_ip_rd_loc_supp_cmd_rp rsp; + struct ble_hs_hci_sup_cmd sup_cmd; + int rc; + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_INFO_PARAMS, + BLE_HCI_OCF_IP_RD_LOC_SUPP_CMD), + NULL, 0, &rsp, sizeof(rsp)); + if (rc != 0) { + return rc; + } + + /* Only check support for Set Event ­Mask ­Page ­2 command */ + sup_cmd.event_mask2 = (rsp.commands[22] & 0x04) != 0; + ble_hs_hci_set_hci_supported_cmd(sup_cmd); + + return 0; +} + static int ble_hs_startup_le_read_sup_f_tx(void) { @@ -311,9 +332,11 @@ ble_hs_startup_set_evmask_tx(void) struct ble_hci_cb_set_event_mask_cp cmd; struct ble_hci_cb_set_event_mask2_cp cmd2; uint8_t version; + struct ble_hs_hci_sup_cmd sup_cmd; int rc; version = ble_hs_hci_get_hci_version(); + sup_cmd = ble_hs_hci_get_hci_supported_cmd(); /** * Enable the following events: @@ -333,7 +356,7 @@ ble_hs_startup_set_evmask_tx(void) return rc; } - if (version >= BLE_HCI_VER_BCS_4_1) { + if ((version >= BLE_HCI_VER_BCS_4_1) && sup_cmd.event_mask2) { /** * Enable the following events: * 0x0000000000800000 Authenticated Payload Timeout Event @@ -374,7 +397,11 @@ ble_hs_startup_go(void) return rc; } - /* XXX: Read local supported commands. */ + /* Read local supported commands. */ + rc = ble_hs_startup_read_sup_cmd_tx(); + if (rc != 0) { + return rc; + } /* we need to check this only if using external controller */ #if !MYNEWT_VAL(BLE_CONTROLLER) diff --git a/nimble/host/test/src/ble_hs_test_util_hci.c b/nimble/host/test/src/ble_hs_test_util_hci.c index e8054c2ff..c27af143a 100644 --- a/nimble/host/test/src/ble_hs_test_util_hci.c +++ b/nimble/host/test/src/ble_hs_test_util_hci.c @@ -250,6 +250,19 @@ static const struct ble_hs_test_util_hci_ack hci_startup_seq[] = { .evt_params = { 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, .evt_params_len = 8, }, + { + .opcode = ble_hs_hci_util_opcode_join( + BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_LOC_SUPP_CMD), + .evt_params = { 0x20, 0x00, 0x80, 0x00, 0x00, 0xc0, 0x00, 0x00, + 0x00, 0x00, 0xe0, 0x00, 0x00, 0x00, 0x28, 0x22, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, + 0x00, 0xf7, 0xff, 0xff, 0x7f, 0x00, 0x00, 0x00, + 0x00, 0xf0, 0xf9, 0xff, 0xff, 0xff, 0xff, 0x07, + 0xe0, 0x63, 0xe0, 0x04, 0x02, 0x00, 0x03, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, + .evt_params_len = 64 + }, { .opcode = ble_hs_hci_util_opcode_join( BLE_HCI_OGF_INFO_PARAMS, BLE_HCI_OCF_IP_RD_LOC_SUPP_FEAT),