From 6b890f81c0db10ae4757e27eb1312f4232d8fb56 Mon Sep 17 00:00:00 2001 From: Rahul Tank Date: Mon, 9 Sep 2024 14:20:49 +0530 Subject: [PATCH] nimble/host: Add support for ext adv param v2 HCI command --- nimble/host/include/host/ble_gap.h | 6 ++ nimble/host/src/ble_gap.c | 152 +++++++++++++++++++++-------- nimble/host/src/ble_hs_hci_priv.h | 2 +- nimble/host/src/ble_hs_startup.c | 5 +- nimble/include/nimble/hci_common.h | 8 ++ 5 files changed, 131 insertions(+), 42 deletions(-) diff --git a/nimble/host/include/host/ble_gap.h b/nimble/host/include/host/ble_gap.h index da4043bdd..6b8fd7baf 100644 --- a/nimble/host/include/host/ble_gap.h +++ b/nimble/host/include/host/ble_gap.h @@ -1678,6 +1678,12 @@ struct ble_gap_ext_adv_params { /** Advertising Set ID */ uint8_t sid; + + /** Primary PHY options */ + uint8_t primary_phy_opt; + + /** Secondary PHY options */ + uint8_t secondary_phy_opt; }; /** diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index 2881ea24e..c5b0f8e8b 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -3360,45 +3360,38 @@ ble_gap_adv_active(void) #if MYNEWT_VAL(BLE_EXT_ADV) static int -ble_gap_ext_adv_params_tx(uint8_t instance, - const struct ble_gap_ext_adv_params *params, - int8_t *selected_tx_power) - +ble_gap_set_ext_adv_params(struct ble_hci_le_set_ext_adv_params_cp *cmd, + uint8_t instance, const struct ble_gap_ext_adv_params *params, + int8_t *selected_tx_power) { - struct ble_hci_le_set_ext_adv_params_cp cmd; - struct ble_hci_le_set_ext_adv_params_rp rsp; - int rc; - - memset(&cmd, 0, sizeof(cmd)); - - cmd.adv_handle = instance; + cmd->adv_handle = instance; if (params->connectable) { - cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE; + cmd->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_CONNECTABLE; } if (params->scannable) { - cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE; + cmd->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_SCANNABLE; } if (params->directed) { - cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED; - cmd.peer_addr_type = params->peer.type; - memcpy(cmd.peer_addr, params->peer.val, BLE_DEV_ADDR_LEN); + cmd->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_DIRECTED; + cmd->peer_addr_type = params->peer.type; + memcpy(cmd->peer_addr, params->peer.val, BLE_DEV_ADDR_LEN); } if (params->high_duty_directed) { - cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED; + cmd->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_HD_DIRECTED; } if (params->anonymous) { - cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV; + cmd->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_ANON_ADV; } if (params->include_tx_power) { - cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR; + cmd->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_INC_TX_PWR; } if (params->legacy_pdu) { - cmd.props |= BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY; + cmd->props |= BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY; /* check right away if the applied configuration is valid before handing * the command to the controller to improve error reporting */ - switch (cmd.props) { + switch (cmd->props) { case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_IND: case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_LD_DIR: case BLE_HCI_LE_SET_EXT_ADV_PROP_LEGACY_HD_DIR: @@ -3413,38 +3406,58 @@ ble_gap_ext_adv_params_tx(uint8_t instance, /* Fill optional fields if application did not specify them. */ if (params->itvl_min == 0 && params->itvl_max == 0) { /* TODO for now limited to legacy values*/ - put_le24(cmd.pri_itvl_min, BLE_GAP_ADV_FAST_INTERVAL1_MIN); - put_le24(cmd.pri_itvl_max, BLE_GAP_ADV_FAST_INTERVAL2_MAX); + put_le24(cmd->pri_itvl_min, BLE_GAP_ADV_FAST_INTERVAL1_MIN); + put_le24(cmd->pri_itvl_max, BLE_GAP_ADV_FAST_INTERVAL2_MAX); } else { - put_le24(cmd.pri_itvl_min, params->itvl_min); - put_le24(cmd.pri_itvl_max, params->itvl_max); + put_le24(cmd->pri_itvl_min, params->itvl_min); + put_le24(cmd->pri_itvl_max, params->itvl_max); } if (params->channel_map == 0) { - cmd.pri_chan_map = BLE_GAP_ADV_DFLT_CHANNEL_MAP; + cmd->pri_chan_map = BLE_GAP_ADV_DFLT_CHANNEL_MAP; } else { - cmd.pri_chan_map = params->channel_map; + cmd->pri_chan_map = params->channel_map; } /* Zero is the default value for filter policy and high duty cycle */ - cmd.filter_policy = params->filter_policy; - cmd.tx_power = params->tx_power; + cmd->filter_policy = params->filter_policy; + cmd->tx_power = params->tx_power; if (params->legacy_pdu) { - cmd.pri_phy = BLE_HCI_LE_PHY_1M; - cmd.sec_phy = BLE_HCI_LE_PHY_1M; + cmd->pri_phy = BLE_HCI_LE_PHY_1M; + cmd->sec_phy = BLE_HCI_LE_PHY_1M; } else { - cmd.pri_phy = params->primary_phy; - cmd.sec_phy = params->secondary_phy; + cmd->pri_phy = params->primary_phy; + cmd->sec_phy = params->secondary_phy; } - cmd.own_addr_type = params->own_addr_type; - cmd.sec_max_skip = 0; - cmd.sid = params->sid; - cmd.scan_req_notif = params->scan_req_notif; + cmd->own_addr_type = params->own_addr_type; + cmd->sec_max_skip = 0; + cmd->sid = params->sid; + cmd->scan_req_notif = params->scan_req_notif; + + return 0; +} + +static int +ble_gap_ext_adv_params_tx_v1(uint8_t instance, + const struct ble_gap_ext_adv_params *params, + int8_t *selected_tx_power) +{ + struct ble_hci_le_set_ext_adv_params_cp cmd; + struct ble_hci_le_set_ext_adv_params_rp rsp; + int rc; + + memset(&cmd, 0, sizeof(cmd)); + + rc = ble_gap_set_ext_adv_params(&cmd, instance, params, selected_tx_power); + + if (rc != 0) { + return rc; + } rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, - BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM), + BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM), &cmd, sizeof(cmd), &rsp, sizeof(rsp)); if (rc != 0) { @@ -3456,6 +3469,69 @@ ble_gap_ext_adv_params_tx(uint8_t instance, } return 0; + +} + +static int +ble_gap_ext_adv_params_tx_v2(uint8_t instance, + const struct ble_gap_ext_adv_params *params, + int8_t *selected_tx_power) +{ + + struct ble_hci_le_set_ext_adv_params_v2_cp cmd; + struct ble_hci_le_set_ext_adv_params_rp rsp; + int rc; + + memset(&cmd, 0, sizeof(cmd)); + + rc = ble_gap_set_ext_adv_params(&(cmd.cmd), instance, params, selected_tx_power); + + if (rc != 0) { + return rc; + } + + cmd.pri_phy_opt = params->primary_phy_opt; + cmd.sec_phy_opt = params->secondary_phy_opt; + + rc = ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, + BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM_V2), + &cmd, sizeof(cmd), &rsp, sizeof(rsp)); + + if (rc != 0) { + return rc; + } + + if (selected_tx_power) { + *selected_tx_power = rsp.tx_power; + } + + return 0; +} + +static int +ble_gap_ext_adv_params_tx(uint8_t instance, + const struct ble_gap_ext_adv_params *params, + int8_t *selected_tx_power) +{ + struct ble_hs_hci_sup_cmd sup_cmd; + int rc = 0; + + sup_cmd = ble_hs_hci_get_hci_supported_cmd(); + + /* Return Error if phy is non-zero and controller doesn't support V2 */ + if (!((sup_cmd.commands[46] & 0x04) != 0) && + (params->primary_phy_opt || params->secondary_phy_opt)) { + return BLE_HS_EINVAL; + } + + if ((sup_cmd.commands[46] & 0x04) != 0) { + rc = ble_gap_ext_adv_params_tx_v2(instance, params, selected_tx_power); + return rc; + } + + rc = ble_gap_ext_adv_params_tx_v1(instance, params, selected_tx_power); + + return rc; } static int diff --git a/nimble/host/src/ble_hs_hci_priv.h b/nimble/host/src/ble_hs_hci_priv.h index 167c598b3..db66c3542 100644 --- a/nimble/host/src/ble_hs_hci_priv.h +++ b/nimble/host/src/ble_hs_hci_priv.h @@ -80,7 +80,7 @@ struct hci_periodic_adv_params #endif struct ble_hs_hci_sup_cmd { - unsigned event_mask2 : 1; /** Indicates whether the controller supports the set event mask page 2 command */ + uint8_t commands[64]; }; extern uint16_t ble_hs_hci_avail_pkts; diff --git a/nimble/host/src/ble_hs_startup.c b/nimble/host/src/ble_hs_startup.c index 96c75251b..3f04325be 100644 --- a/nimble/host/src/ble_hs_startup.c +++ b/nimble/host/src/ble_hs_startup.c @@ -82,8 +82,7 @@ ble_hs_startup_read_sup_cmd_tx(void) return rc; } - /* Only check support for Set Event ­Mask ­Page ­2 command */ - sup_cmd.event_mask2 = (rsp.commands[22] & 0x04) != 0; + memcpy(&sup_cmd.commands, &rsp.commands, sizeof(sup_cmd)); ble_hs_hci_set_hci_supported_cmd(sup_cmd); return 0; @@ -356,7 +355,7 @@ ble_hs_startup_set_evmask_tx(void) return rc; } - if ((version >= BLE_HCI_VER_BCS_4_1) && sup_cmd.event_mask2) { + if ((version >= BLE_HCI_VER_BCS_4_1) && ((sup_cmd.commands[22] & 0x04) != 0)) { /** * Enable the following events: * 0x0000000000800000 Authenticated Payload Timeout Event diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h index 6a12bfcf6..a6bf52e4c 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -600,6 +600,7 @@ struct ble_hci_le_set_ext_adv_params_cp { uint8_t sid; uint8_t scan_req_notif; } __attribute__((packed)); + struct ble_hci_le_set_ext_adv_params_rp { int8_t tx_power; } __attribute__((packed)); @@ -1235,6 +1236,13 @@ struct ble_hci_le_subrate_req_cp { uint16_t supervision_tmo; } __attribute__((packed)); +#define BLE_HCI_OCF_LE_SET_EXT_ADV_PARAM_V2 (0x007F) +struct ble_hci_le_set_ext_adv_params_v2_cp { + struct ble_hci_le_set_ext_adv_params_cp cmd; + uint8_t pri_phy_opt; + uint8_t sec_phy_opt; +} __attribute__((packed)); + /* --- Vendor specific commands (OGF 0x003F) */ /* Read Random Static Address */ #define BLE_HCI_OCF_VS_RD_STATIC_ADDR (MYNEWT_VAL(BLE_HCI_VS_OCF_OFFSET) + (0x0001))