diff --git a/nimble/host/include/host/ble_gap.h b/nimble/host/include/host/ble_gap.h index e842ae29a7..33c56d76db 100644 --- a/nimble/host/include/host/ble_gap.h +++ b/nimble/host/include/host/ble_gap.h @@ -1582,6 +1582,12 @@ struct ble_gap_ext_adv_params { /** Advertising Set ID */ uint8_t sid; + + /** Primary phy options */ + uint8_t pri_phy_opt; + + /** Secondary phy options */ + uint8_t sec_phy_opt; }; /** diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c index 38da71200b..f46fda68e8 100644 --- a/nimble/host/src/ble_gap.c +++ b/nimble/host/src/ble_gap.c @@ -3124,45 +3124,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: @@ -3177,38 +3170,95 @@ 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; + + 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), + &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_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.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.pri_phy_opt = params->pri_phy_opt; + cmd.sec_phy_opt = params->sec_phy_opt; 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_V2), &cmd, sizeof(cmd), &rsp, sizeof(rsp)); if (rc != 0) { @@ -3222,6 +3272,32 @@ ble_gap_ext_adv_params_tx(uint8_t instance, 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 || params->secondary_phy)) { + 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 ble_gap_ext_adv_params_validate(const struct ble_gap_ext_adv_params *params) { diff --git a/nimble/host/src/ble_hs_hci_priv.h b/nimble/host/src/ble_hs_hci_priv.h index 78621e046a..211f3f931c 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 c23579547c..2f95880be9 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; @@ -373,7 +372,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 b1f6261266..68f1538826 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -593,6 +593,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)); @@ -1149,6 +1150,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)); + #define BLE_HCI_OCF_LE_CS_RD_LOC_SUPP_CAP (0x0089) struct ble_hci_le_cs_rd_loc_supp_cap_rp { uint8_t num_config_supported;