andrzej-kaczmarek commented on code in PR #1765:
URL: https://github.com/apache/mynewt-nimble/pull/1765#discussion_r1634382957
##########
nimble/controller/src/ble_ll_cs.c:
##########
@@ -26,75 +26,973 @@
#include "controller/ble_ll_conn.h"
#include "controller/ble_ll_hci.h"
#include "controller/ble_ll_cs.h"
+#include "ble_ll_conn_priv.h"
+#include "ble_ll_cs_priv.h"
+
+static struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap;
+static struct ble_ll_cs_sm g_ble_ll_cs_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)];
+static const uint8_t t_ip1[] = {10, 20, 30, 40, 50, 60, 80, 145};
+static const uint8_t t_ip2[] = {10, 20, 30, 40, 50, 60, 80, 145};
+static const uint8_t t_fcs[] = {15, 20, 30, 40, 50, 60, 80, 100, 120, 150};
+static const uint8_t t_pm[] = {10, 20, 40};
+static const uint8_t default_channel_classification[10] = {
+ 0xFC, 0xFF, 0x7F, 0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F
+};
+static uint8_t g_ble_ll_cs_chan_class[10];
+static uint8_t g_ble_ll_cs_chan_count = 0;
+static uint8_t g_ble_ll_cs_chan_indices[72];
int
ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+ struct ble_hci_le_cs_rd_loc_supp_cap_rp *rsp = (void *)rspbuf;
+
+ rsp->num_config_supported = cap->max_number_of_configs;
+ rsp->max_consecutive_procedures_supported =
htole16(cap->max_number_of_procedures);
+ rsp->num_antennas_supported = cap->number_of_antennas;
+ rsp->max_antenna_paths_supported = cap->max_number_of_antenna_paths;
+ rsp->roles_supported = cap->roles_supported;
+ rsp->optional_modes_supported = cap->mode_types;
+ rsp->rtt_capability = cap->rtt_capability;
+ rsp->rtt_aa_only_n = cap->rtt_aa_only_n;
+ rsp->rtt_sounding_n = cap->rtt_sounding_n;
+ rsp->rtt_random_payload_n = cap->rtt_random_sequence_n;
+ rsp->optional_nadm_sounding_capability =
htole16(cap->nadm_sounding_capability);
+ rsp->optional_nadm_random_capability =
htole16(cap->nadm_random_sequence_capability);
+ rsp->optional_cs_sync_phys_supported = cap->cs_sync_phy_capability;
+ rsp->optional_subfeatures_supported = htole16(0x000f &
+ (cap->no_fae << 1 |
+ cap->channel_selection << 2
|
+ cap->sounding_pct_estimate
<< 3));
+ rsp->optional_t_ip1_times_supported = htole16(cap->t_ip1_capability);
+ rsp->optional_t_ip2_times_supported = htole16(cap->t_ip2_capability);
+ rsp->optional_t_fcs_times_supported = htole16(cap->t_fcs_capability);
+ rsp->optional_t_pm_times_supported = htole16(cap->t_pm_capability);
+ rsp->t_sw_time_supported = cap->t_sw;
+ rsp->optional_tx_snr_capability = cap->tx_snr_capablity;
+
+ *rsplen = sizeof(*rsp);
+
+ return BLE_ERR_SUCCESS;
+}
+
+void
+ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+
+ *dptr = cap->mode_types;
+ dptr[1] = cap->rtt_capability;
+ dptr[2] = cap->rtt_aa_only_n;
+ dptr[3] = cap->rtt_sounding_n;
+ dptr[4] = cap->rtt_random_sequence_n;
+ put_le16(dptr + 5, cap->nadm_sounding_capability);
+ put_le16(dptr + 7, cap->nadm_random_sequence_capability);
+ dptr[9] = cap->cs_sync_phy_capability;
+ dptr[10] = cap->number_of_antennas | cap->max_number_of_antenna_paths << 4;
+ dptr[11] = cap->roles_supported |
+ cap->no_fae << 3 |
+ cap->channel_selection << 4 |
+ cap->sounding_pct_estimate << 5;
+ dptr[12] = cap->max_number_of_configs;
+ put_le16(dptr + 13, cap->max_number_of_procedures);
+ dptr[15] = cap->t_sw;
+ put_le16(dptr + 16, cap->t_ip1_capability);
+ put_le16(dptr + 18, cap->t_ip2_capability);
+ put_le16(dptr + 20, cap->t_fcs_capability);
+ put_le16(dptr + 22, cap->t_pm_capability);
+ dptr[24] = cap->tx_snr_capablity << 1;
+}
+
+static void
+ble_ll_cs_update_rem_capabilities(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ struct ble_ll_cs_supp_cap *cap = &connsm->cssm->remote_cap;
+
+ cap->mode_types = *dptr;
+ cap->rtt_capability = dptr[1];
+ cap->rtt_aa_only_n = dptr[2];
+ cap->rtt_sounding_n = dptr[3];
+ cap->rtt_random_sequence_n = dptr[4];
+ cap->nadm_sounding_capability = get_le16(dptr + 5);
+ cap->nadm_random_sequence_capability = get_le16(dptr + 7);
+ cap->cs_sync_phy_capability = dptr[9];
+
+ cap->number_of_antennas = dptr[10] & 0b00001111;
+ cap->max_number_of_antenna_paths = dptr[10] >> 4;
+
+ cap->roles_supported = dptr[11] & 0b00000011;
+ cap->no_fae = (dptr[11] & 0b00001000) >> 3;
+ cap->channel_selection = (dptr[11] & 0b00010000) >> 4;
+ cap->sounding_pct_estimate = (dptr[11] & 0b00100000) >> 5;
+
+ cap->max_number_of_configs = dptr[12];
+ cap->max_number_of_procedures = get_le16(dptr + 13);
+ cap->t_sw = dptr[15];
+ cap->t_ip1_capability = get_le16(dptr + 16);
+ cap->t_ip2_capability = get_le16(dptr + 18);
+ cap->t_fcs_capability = get_le16(dptr + 20);
+ cap->t_pm_capability = get_le16(dptr + 22);
+ cap->tx_snr_capablity = (dptr[24] >> 1) & 0b01111111;
+}
+
+static void
+ble_ll_cs_ev_rd_rem_supp_cap(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+ const struct ble_ll_cs_supp_cap *cap = &connsm->cssm->remote_cap;
+ struct ble_hci_ev_le_subev_cs_rd_rem_supp_cap_complete *ev;
+ struct ble_hci_ev *hci_ev;
+
+ if (ble_ll_hci_is_le_event_enabled(
+ BLE_HCI_LE_SUBEV_CS_RD_REM_SUPP_CAP_COMPLETE)) {
+ hci_ev = ble_transport_alloc_evt(0);
+ if (hci_ev) {
+ hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+ hci_ev->length = sizeof(*ev);
+ ev = (void *) hci_ev->data;
+
+ ev->subev_code = BLE_HCI_LE_SUBEV_CS_RD_REM_SUPP_CAP_COMPLETE;
+ ev->status = status;
+ ev->conn_handle = htole16(connsm->conn_handle);
+
+ ev->num_config_supported = cap->max_number_of_configs;
+ ev->max_consecutive_procedures_supported =
htole16(cap->max_number_of_procedures);
+ ev->num_antennas_supported = cap->number_of_antennas;
+ ev->max_antenna_paths_supported = cap->max_number_of_antenna_paths;
+ ev->roles_supported = cap->roles_supported;
+ ev->optional_modes_supported = cap->mode_types;
+ ev->rtt_capability = cap->rtt_capability;
+ ev->rtt_aa_only_n = cap->rtt_aa_only_n;
+ ev->rtt_sounding_n = cap->rtt_sounding_n;
+ ev->rtt_random_payload_n = cap->rtt_random_sequence_n;
+ ev->optional_nadm_sounding_capability =
htole16(cap->nadm_sounding_capability);
+ ev->optional_nadm_random_capability =
htole16(cap->nadm_random_sequence_capability);
+ ev->optional_cs_sync_phys_supported = cap->cs_sync_phy_capability;
+ ev->optional_subfeatures_supported = htole16(cap->no_fae << 1 |
+
cap->channel_selection << 2 |
+
cap->sounding_pct_estimate << 3);
+ ev->optional_t_ip1_times_supported =
htole16(cap->t_ip1_capability);
+ ev->optional_t_ip2_times_supported =
htole16(cap->t_ip2_capability);
+ ev->optional_t_fcs_times_supported =
htole16(cap->t_fcs_capability);
+ ev->optional_t_pm_times_supported = htole16(cap->t_pm_capability);
+ ev->t_sw_time_supported = cap->t_sw;
+ ev->optional_tx_snr_capability = cap->tx_snr_capablity;
+
+ ble_ll_hci_event_send(hci_ev);
+ }
+ }
+}
+
+int
+ble_ll_cs_rx_capabilities_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+ uint8_t *rspbuf)
+{
+ ble_ll_cs_update_rem_capabilities(connsm, dptr);
+
+ ble_ll_cs_capabilities_pdu_make(connsm, rspbuf);
+
+ return BLE_LL_CTRL_CS_CAPABILITIES_RSP;
+}
+
+void
+ble_ll_cs_rx_capabilities_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ ble_ll_cs_update_rem_capabilities(connsm, dptr);
Review Comment:
this should verify data in rsp and return error if needed
##########
nimble/controller/src/ble_ll_cs.c:
##########
@@ -26,75 +26,973 @@
#include "controller/ble_ll_conn.h"
#include "controller/ble_ll_hci.h"
#include "controller/ble_ll_cs.h"
+#include "ble_ll_conn_priv.h"
+#include "ble_ll_cs_priv.h"
+
+static struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap;
+static struct ble_ll_cs_sm g_ble_ll_cs_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)];
+static const uint8_t t_ip1[] = {10, 20, 30, 40, 50, 60, 80, 145};
+static const uint8_t t_ip2[] = {10, 20, 30, 40, 50, 60, 80, 145};
+static const uint8_t t_fcs[] = {15, 20, 30, 40, 50, 60, 80, 100, 120, 150};
+static const uint8_t t_pm[] = {10, 20, 40};
+static const uint8_t default_channel_classification[10] = {
+ 0xFC, 0xFF, 0x7F, 0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F
+};
+static uint8_t g_ble_ll_cs_chan_class[10];
+static uint8_t g_ble_ll_cs_chan_count = 0;
+static uint8_t g_ble_ll_cs_chan_indices[72];
int
ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+ struct ble_hci_le_cs_rd_loc_supp_cap_rp *rsp = (void *)rspbuf;
+
+ rsp->num_config_supported = cap->max_number_of_configs;
+ rsp->max_consecutive_procedures_supported =
htole16(cap->max_number_of_procedures);
+ rsp->num_antennas_supported = cap->number_of_antennas;
+ rsp->max_antenna_paths_supported = cap->max_number_of_antenna_paths;
+ rsp->roles_supported = cap->roles_supported;
+ rsp->optional_modes_supported = cap->mode_types;
+ rsp->rtt_capability = cap->rtt_capability;
+ rsp->rtt_aa_only_n = cap->rtt_aa_only_n;
+ rsp->rtt_sounding_n = cap->rtt_sounding_n;
+ rsp->rtt_random_payload_n = cap->rtt_random_sequence_n;
+ rsp->optional_nadm_sounding_capability =
htole16(cap->nadm_sounding_capability);
+ rsp->optional_nadm_random_capability =
htole16(cap->nadm_random_sequence_capability);
+ rsp->optional_cs_sync_phys_supported = cap->cs_sync_phy_capability;
+ rsp->optional_subfeatures_supported = htole16(0x000f &
+ (cap->no_fae << 1 |
+ cap->channel_selection << 2
|
+ cap->sounding_pct_estimate
<< 3));
+ rsp->optional_t_ip1_times_supported = htole16(cap->t_ip1_capability);
+ rsp->optional_t_ip2_times_supported = htole16(cap->t_ip2_capability);
+ rsp->optional_t_fcs_times_supported = htole16(cap->t_fcs_capability);
+ rsp->optional_t_pm_times_supported = htole16(cap->t_pm_capability);
+ rsp->t_sw_time_supported = cap->t_sw;
+ rsp->optional_tx_snr_capability = cap->tx_snr_capablity;
+
+ *rsplen = sizeof(*rsp);
+
+ return BLE_ERR_SUCCESS;
+}
+
+void
+ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+
+ *dptr = cap->mode_types;
+ dptr[1] = cap->rtt_capability;
+ dptr[2] = cap->rtt_aa_only_n;
+ dptr[3] = cap->rtt_sounding_n;
+ dptr[4] = cap->rtt_random_sequence_n;
+ put_le16(dptr + 5, cap->nadm_sounding_capability);
+ put_le16(dptr + 7, cap->nadm_random_sequence_capability);
+ dptr[9] = cap->cs_sync_phy_capability;
+ dptr[10] = cap->number_of_antennas | cap->max_number_of_antenna_paths << 4;
+ dptr[11] = cap->roles_supported |
+ cap->no_fae << 3 |
+ cap->channel_selection << 4 |
+ cap->sounding_pct_estimate << 5;
+ dptr[12] = cap->max_number_of_configs;
+ put_le16(dptr + 13, cap->max_number_of_procedures);
+ dptr[15] = cap->t_sw;
+ put_le16(dptr + 16, cap->t_ip1_capability);
+ put_le16(dptr + 18, cap->t_ip2_capability);
+ put_le16(dptr + 20, cap->t_fcs_capability);
+ put_le16(dptr + 22, cap->t_pm_capability);
+ dptr[24] = cap->tx_snr_capablity << 1;
+}
+
+static void
+ble_ll_cs_update_rem_capabilities(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ struct ble_ll_cs_supp_cap *cap = &connsm->cssm->remote_cap;
+
+ cap->mode_types = *dptr;
+ cap->rtt_capability = dptr[1];
+ cap->rtt_aa_only_n = dptr[2];
+ cap->rtt_sounding_n = dptr[3];
+ cap->rtt_random_sequence_n = dptr[4];
+ cap->nadm_sounding_capability = get_le16(dptr + 5);
+ cap->nadm_random_sequence_capability = get_le16(dptr + 7);
+ cap->cs_sync_phy_capability = dptr[9];
+
+ cap->number_of_antennas = dptr[10] & 0b00001111;
+ cap->max_number_of_antenna_paths = dptr[10] >> 4;
+
+ cap->roles_supported = dptr[11] & 0b00000011;
+ cap->no_fae = (dptr[11] & 0b00001000) >> 3;
+ cap->channel_selection = (dptr[11] & 0b00010000) >> 4;
+ cap->sounding_pct_estimate = (dptr[11] & 0b00100000) >> 5;
+
+ cap->max_number_of_configs = dptr[12];
+ cap->max_number_of_procedures = get_le16(dptr + 13);
+ cap->t_sw = dptr[15];
+ cap->t_ip1_capability = get_le16(dptr + 16);
+ cap->t_ip2_capability = get_le16(dptr + 18);
+ cap->t_fcs_capability = get_le16(dptr + 20);
+ cap->t_pm_capability = get_le16(dptr + 22);
+ cap->tx_snr_capablity = (dptr[24] >> 1) & 0b01111111;
+}
+
+static void
+ble_ll_cs_ev_rd_rem_supp_cap(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+ const struct ble_ll_cs_supp_cap *cap = &connsm->cssm->remote_cap;
+ struct ble_hci_ev_le_subev_cs_rd_rem_supp_cap_complete *ev;
+ struct ble_hci_ev *hci_ev;
+
+ if (ble_ll_hci_is_le_event_enabled(
+ BLE_HCI_LE_SUBEV_CS_RD_REM_SUPP_CAP_COMPLETE)) {
+ hci_ev = ble_transport_alloc_evt(0);
+ if (hci_ev) {
+ hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+ hci_ev->length = sizeof(*ev);
+ ev = (void *) hci_ev->data;
+
+ ev->subev_code = BLE_HCI_LE_SUBEV_CS_RD_REM_SUPP_CAP_COMPLETE;
+ ev->status = status;
+ ev->conn_handle = htole16(connsm->conn_handle);
+
+ ev->num_config_supported = cap->max_number_of_configs;
+ ev->max_consecutive_procedures_supported =
htole16(cap->max_number_of_procedures);
+ ev->num_antennas_supported = cap->number_of_antennas;
+ ev->max_antenna_paths_supported = cap->max_number_of_antenna_paths;
+ ev->roles_supported = cap->roles_supported;
+ ev->optional_modes_supported = cap->mode_types;
+ ev->rtt_capability = cap->rtt_capability;
+ ev->rtt_aa_only_n = cap->rtt_aa_only_n;
+ ev->rtt_sounding_n = cap->rtt_sounding_n;
+ ev->rtt_random_payload_n = cap->rtt_random_sequence_n;
+ ev->optional_nadm_sounding_capability =
htole16(cap->nadm_sounding_capability);
+ ev->optional_nadm_random_capability =
htole16(cap->nadm_random_sequence_capability);
+ ev->optional_cs_sync_phys_supported = cap->cs_sync_phy_capability;
+ ev->optional_subfeatures_supported = htole16(cap->no_fae << 1 |
+
cap->channel_selection << 2 |
+
cap->sounding_pct_estimate << 3);
+ ev->optional_t_ip1_times_supported =
htole16(cap->t_ip1_capability);
+ ev->optional_t_ip2_times_supported =
htole16(cap->t_ip2_capability);
+ ev->optional_t_fcs_times_supported =
htole16(cap->t_fcs_capability);
+ ev->optional_t_pm_times_supported = htole16(cap->t_pm_capability);
+ ev->t_sw_time_supported = cap->t_sw;
+ ev->optional_tx_snr_capability = cap->tx_snr_capablity;
+
+ ble_ll_hci_event_send(hci_ev);
+ }
+ }
+}
+
+int
+ble_ll_cs_rx_capabilities_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+ uint8_t *rspbuf)
+{
+ ble_ll_cs_update_rem_capabilities(connsm, dptr);
+
+ ble_ll_cs_capabilities_pdu_make(connsm, rspbuf);
+
+ return BLE_LL_CTRL_CS_CAPABILITIES_RSP;
+}
+
+void
+ble_ll_cs_rx_capabilities_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ ble_ll_cs_update_rem_capabilities(connsm, dptr);
+
+ if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG)) {
+ /* Should never happen */
+ return;
+ }
+
+ /* Stop the control procedure and send an event to the host */
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG);
+ ble_ll_cs_ev_rd_rem_supp_cap(connsm, BLE_ERR_SUCCESS);
}
int
ble_ll_cs_hci_rd_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_rd_rem_supp_cap_cp *cmd = (const void *)cmdbuf;
+ struct ble_ll_conn_sm *connsm;
+
+ if (cmdlen != sizeof(*cmd)) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ /* If no connection handle exit with error */
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ /* If already pending exit with error */
+ if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG)) {
+ return BLE_ERR_CMD_DISALLOWED;
+ }
+
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG, NULL);
+
+ return BLE_ERR_SUCCESS;
}
int
ble_ll_cs_hci_wr_cached_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen,
uint8_t *rspbuf, uint8_t *rsplen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_wr_cached_rem_supp_cap_cp *cmd = (const void
*)cmdbuf;
+ struct ble_hci_le_cs_wr_cached_rem_supp_cap_rp *rsp = (void *)rspbuf;
+ struct ble_ll_cs_supp_cap *cap;
+ struct ble_ll_conn_sm *connsm;
+ uint16_t subfeatures;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ cap = &connsm->cssm->remote_cap;
+
+ cap->max_number_of_configs = cmd->num_config_supported;
+ cap->max_number_of_procedures =
le16toh(cmd->max_consecutive_procedures_supported);
+ cap->number_of_antennas = cmd->num_antennas_supported;
+ cap->max_number_of_antenna_paths = cmd->max_antenna_paths_supported;
+ cap->roles_supported = cmd->roles_supported;
+ cap->mode_types = cmd->optional_modes_supported;
+ cap->rtt_capability = cmd->rtt_capability;
+ cap->rtt_aa_only_n = cmd->rtt_aa_only_n;
+ cap->rtt_sounding_n = cmd->rtt_sounding_n;
+ cap->rtt_random_sequence_n = cmd->rtt_random_payload_n;
+ cap->nadm_sounding_capability =
le16toh(cmd->optional_nadm_sounding_capability);
+ cap->nadm_random_sequence_capability =
le16toh(cmd->optional_nadm_random_capability);
+ cap->cs_sync_phy_capability = cmd->optional_cs_sync_phys_supported;
+
+ subfeatures = le16toh(cmd->optional_subfeatures_supported);
+ cap->no_fae = (subfeatures >> 1) & 1;
+ cap->channel_selection = (subfeatures >> 2) & 1;
+ cap->sounding_pct_estimate = (subfeatures >> 3) & 1;
+
+ cap->t_ip1_capability = le16toh(cmd->optional_t_ip1_times_supported);
+ cap->t_ip2_capability = le16toh(cmd->optional_t_ip2_times_supported);
+ cap->t_fcs_capability = le16toh(cmd->optional_t_fcs_times_supported);
+ cap->t_pm_capability = le16toh(cmd->optional_t_pm_times_supported);
+ cap->t_sw = cmd->t_sw_time_supported;
+ cap->tx_snr_capablity = cmd->optional_tx_snr_capability;
+
+ rsp->conn_handle = cmd->conn_handle;
+ *rsplen = sizeof(*rsp);
+
+ return BLE_ERR_SUCCESS;
+}
+
+static int
+rej_ext_ind_make(uint8_t rej_opcode, uint8_t err, uint8_t *ctrdata)
+{
+ ctrdata[0] = rej_opcode;
+ ctrdata[1] = err;
+
+ return BLE_LL_CTRL_REJECT_IND_EXT;
+}
+
+int
+ble_ll_cs_rx_security_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+ uint8_t *rspbuf)
+{
+ uint8_t *iv = connsm->cssm->drbg_ctx.iv;
+ uint8_t *in = connsm->cssm->drbg_ctx.in;
+ uint8_t *pv = connsm->cssm->drbg_ctx.pv;
+
+ if (!connsm->flags.encrypted) {
+ return rej_ext_ind_make(BLE_LL_CTRL_CS_SEC_REQ,
+ BLE_ERR_INSUFFICIENT_SEC, rspbuf);
+ }
+
+ /* Vectors concatenation is done in the follwing manner:
+ * CS_IV = CS_IV_P || CS_IV_C
+ * The CS_IV_C is concatenated with the CS_IV_P. The least significant
+ * octet of CS_IV_C becomes the least significant octet of CS_IV. The most
+ * significant octet of CS_IV_P becomes the most significant octet of
CS_IV.
+ */
+
+ /* Save Central's vector */
+ memcpy(iv, dptr, 8);
+ memcpy(in, dptr + 8, 4);
+ memcpy(pv, dptr + 12, 8);
+
+ /* Generate Peripheral's vector */
+ ble_ll_rand_data_get(iv + 8, 8);
+ ble_ll_rand_data_get(in + 4, 4);
+ ble_ll_rand_data_get(pv + 8, 8);
+
+ memcpy(rspbuf, iv + 8, 8);
+ memcpy(rspbuf + 8, in + 4, 4);
+ memcpy(rspbuf + 12, pv + 8, 8);
+
+ ble_ll_cs_drbg_init(&connsm->cssm->drbg_ctx);
+
+ return BLE_LL_CTRL_CS_SEC_RSP;
+}
+
+static void
+ble_ll_cs_ev_sec_enable_complete(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+ struct ble_hci_ev_le_subev_cs_sec_enable_complete *ev;
+ struct ble_hci_ev *hci_ev;
+
+ if (ble_ll_hci_is_le_event_enabled(
+ BLE_HCI_LE_SUBEV_CS_SEC_ENABLE_COMPLETE)) {
+ hci_ev = ble_transport_alloc_evt(0);
+ if (hci_ev) {
+ hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+ hci_ev->length = sizeof(*ev);
+ ev = (void *) hci_ev->data;
+
+ ev->subev_code = BLE_HCI_LE_SUBEV_CS_SEC_ENABLE_COMPLETE;
+ ev->status = status;
+ ev->conn_handle = htole16(connsm->conn_handle);
+
+ ble_ll_hci_event_send(hci_ev);
+ }
+ }
+}
+
+void
+ble_ll_cs_rx_security_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ int rc = 0;
+ struct ble_ll_cs_drbg_ctx *drbg_ctx = &connsm->cssm->drbg_ctx;
+
+ if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_SEC_START)) {
+ /* Should never happen */
Review Comment:
in bluetooth it will happen ;)
just change the comment to "ignore" or smth
##########
nimble/controller/src/ble_ll_cs.c:
##########
@@ -26,75 +26,973 @@
#include "controller/ble_ll_conn.h"
#include "controller/ble_ll_hci.h"
#include "controller/ble_ll_cs.h"
+#include "ble_ll_conn_priv.h"
+#include "ble_ll_cs_priv.h"
+
+static struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap;
+static struct ble_ll_cs_sm g_ble_ll_cs_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)];
+static const uint8_t t_ip1[] = {10, 20, 30, 40, 50, 60, 80, 145};
+static const uint8_t t_ip2[] = {10, 20, 30, 40, 50, 60, 80, 145};
+static const uint8_t t_fcs[] = {15, 20, 30, 40, 50, 60, 80, 100, 120, 150};
+static const uint8_t t_pm[] = {10, 20, 40};
+static const uint8_t default_channel_classification[10] = {
+ 0xFC, 0xFF, 0x7F, 0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F
+};
+static uint8_t g_ble_ll_cs_chan_class[10];
+static uint8_t g_ble_ll_cs_chan_count = 0;
+static uint8_t g_ble_ll_cs_chan_indices[72];
int
ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+ struct ble_hci_le_cs_rd_loc_supp_cap_rp *rsp = (void *)rspbuf;
+
+ rsp->num_config_supported = cap->max_number_of_configs;
+ rsp->max_consecutive_procedures_supported =
htole16(cap->max_number_of_procedures);
+ rsp->num_antennas_supported = cap->number_of_antennas;
+ rsp->max_antenna_paths_supported = cap->max_number_of_antenna_paths;
+ rsp->roles_supported = cap->roles_supported;
+ rsp->optional_modes_supported = cap->mode_types;
+ rsp->rtt_capability = cap->rtt_capability;
+ rsp->rtt_aa_only_n = cap->rtt_aa_only_n;
+ rsp->rtt_sounding_n = cap->rtt_sounding_n;
+ rsp->rtt_random_payload_n = cap->rtt_random_sequence_n;
+ rsp->optional_nadm_sounding_capability =
htole16(cap->nadm_sounding_capability);
+ rsp->optional_nadm_random_capability =
htole16(cap->nadm_random_sequence_capability);
+ rsp->optional_cs_sync_phys_supported = cap->cs_sync_phy_capability;
+ rsp->optional_subfeatures_supported = htole16(0x000f &
+ (cap->no_fae << 1 |
+ cap->channel_selection << 2
|
+ cap->sounding_pct_estimate
<< 3));
+ rsp->optional_t_ip1_times_supported = htole16(cap->t_ip1_capability);
+ rsp->optional_t_ip2_times_supported = htole16(cap->t_ip2_capability);
+ rsp->optional_t_fcs_times_supported = htole16(cap->t_fcs_capability);
+ rsp->optional_t_pm_times_supported = htole16(cap->t_pm_capability);
+ rsp->t_sw_time_supported = cap->t_sw;
+ rsp->optional_tx_snr_capability = cap->tx_snr_capablity;
+
+ *rsplen = sizeof(*rsp);
+
+ return BLE_ERR_SUCCESS;
+}
+
+void
+ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+
+ *dptr = cap->mode_types;
+ dptr[1] = cap->rtt_capability;
+ dptr[2] = cap->rtt_aa_only_n;
+ dptr[3] = cap->rtt_sounding_n;
+ dptr[4] = cap->rtt_random_sequence_n;
+ put_le16(dptr + 5, cap->nadm_sounding_capability);
+ put_le16(dptr + 7, cap->nadm_random_sequence_capability);
+ dptr[9] = cap->cs_sync_phy_capability;
+ dptr[10] = cap->number_of_antennas | cap->max_number_of_antenna_paths << 4;
+ dptr[11] = cap->roles_supported |
+ cap->no_fae << 3 |
+ cap->channel_selection << 4 |
+ cap->sounding_pct_estimate << 5;
+ dptr[12] = cap->max_number_of_configs;
+ put_le16(dptr + 13, cap->max_number_of_procedures);
+ dptr[15] = cap->t_sw;
+ put_le16(dptr + 16, cap->t_ip1_capability);
+ put_le16(dptr + 18, cap->t_ip2_capability);
+ put_le16(dptr + 20, cap->t_fcs_capability);
+ put_le16(dptr + 22, cap->t_pm_capability);
+ dptr[24] = cap->tx_snr_capablity << 1;
+}
+
+static void
+ble_ll_cs_update_rem_capabilities(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ struct ble_ll_cs_supp_cap *cap = &connsm->cssm->remote_cap;
+
+ cap->mode_types = *dptr;
+ cap->rtt_capability = dptr[1];
+ cap->rtt_aa_only_n = dptr[2];
+ cap->rtt_sounding_n = dptr[3];
+ cap->rtt_random_sequence_n = dptr[4];
+ cap->nadm_sounding_capability = get_le16(dptr + 5);
+ cap->nadm_random_sequence_capability = get_le16(dptr + 7);
+ cap->cs_sync_phy_capability = dptr[9];
+
+ cap->number_of_antennas = dptr[10] & 0b00001111;
+ cap->max_number_of_antenna_paths = dptr[10] >> 4;
+
+ cap->roles_supported = dptr[11] & 0b00000011;
+ cap->no_fae = (dptr[11] & 0b00001000) >> 3;
+ cap->channel_selection = (dptr[11] & 0b00010000) >> 4;
+ cap->sounding_pct_estimate = (dptr[11] & 0b00100000) >> 5;
+
+ cap->max_number_of_configs = dptr[12];
+ cap->max_number_of_procedures = get_le16(dptr + 13);
+ cap->t_sw = dptr[15];
+ cap->t_ip1_capability = get_le16(dptr + 16);
+ cap->t_ip2_capability = get_le16(dptr + 18);
+ cap->t_fcs_capability = get_le16(dptr + 20);
+ cap->t_pm_capability = get_le16(dptr + 22);
+ cap->tx_snr_capablity = (dptr[24] >> 1) & 0b01111111;
+}
+
+static void
+ble_ll_cs_ev_rd_rem_supp_cap(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+ const struct ble_ll_cs_supp_cap *cap = &connsm->cssm->remote_cap;
+ struct ble_hci_ev_le_subev_cs_rd_rem_supp_cap_complete *ev;
+ struct ble_hci_ev *hci_ev;
+
+ if (ble_ll_hci_is_le_event_enabled(
+ BLE_HCI_LE_SUBEV_CS_RD_REM_SUPP_CAP_COMPLETE)) {
+ hci_ev = ble_transport_alloc_evt(0);
+ if (hci_ev) {
+ hci_ev->opcode = BLE_HCI_EVCODE_LE_META;
+ hci_ev->length = sizeof(*ev);
+ ev = (void *) hci_ev->data;
+
+ ev->subev_code = BLE_HCI_LE_SUBEV_CS_RD_REM_SUPP_CAP_COMPLETE;
+ ev->status = status;
+ ev->conn_handle = htole16(connsm->conn_handle);
+
+ ev->num_config_supported = cap->max_number_of_configs;
+ ev->max_consecutive_procedures_supported =
htole16(cap->max_number_of_procedures);
+ ev->num_antennas_supported = cap->number_of_antennas;
+ ev->max_antenna_paths_supported = cap->max_number_of_antenna_paths;
+ ev->roles_supported = cap->roles_supported;
+ ev->optional_modes_supported = cap->mode_types;
+ ev->rtt_capability = cap->rtt_capability;
+ ev->rtt_aa_only_n = cap->rtt_aa_only_n;
+ ev->rtt_sounding_n = cap->rtt_sounding_n;
+ ev->rtt_random_payload_n = cap->rtt_random_sequence_n;
+ ev->optional_nadm_sounding_capability =
htole16(cap->nadm_sounding_capability);
+ ev->optional_nadm_random_capability =
htole16(cap->nadm_random_sequence_capability);
+ ev->optional_cs_sync_phys_supported = cap->cs_sync_phy_capability;
+ ev->optional_subfeatures_supported = htole16(cap->no_fae << 1 |
+
cap->channel_selection << 2 |
+
cap->sounding_pct_estimate << 3);
+ ev->optional_t_ip1_times_supported =
htole16(cap->t_ip1_capability);
+ ev->optional_t_ip2_times_supported =
htole16(cap->t_ip2_capability);
+ ev->optional_t_fcs_times_supported =
htole16(cap->t_fcs_capability);
+ ev->optional_t_pm_times_supported = htole16(cap->t_pm_capability);
+ ev->t_sw_time_supported = cap->t_sw;
+ ev->optional_tx_snr_capability = cap->tx_snr_capablity;
+
+ ble_ll_hci_event_send(hci_ev);
+ }
+ }
+}
+
+int
+ble_ll_cs_rx_capabilities_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+ uint8_t *rspbuf)
+{
+ ble_ll_cs_update_rem_capabilities(connsm, dptr);
+
+ ble_ll_cs_capabilities_pdu_make(connsm, rspbuf);
+
+ return BLE_LL_CTRL_CS_CAPABILITIES_RSP;
+}
+
+void
+ble_ll_cs_rx_capabilities_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ ble_ll_cs_update_rem_capabilities(connsm, dptr);
+
+ if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG)) {
+ /* Should never happen */
+ return;
+ }
+
+ /* Stop the control procedure and send an event to the host */
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG);
+ ble_ll_cs_ev_rd_rem_supp_cap(connsm, BLE_ERR_SUCCESS);
}
int
ble_ll_cs_hci_rd_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_rd_rem_supp_cap_cp *cmd = (const void *)cmdbuf;
+ struct ble_ll_conn_sm *connsm;
+
+ if (cmdlen != sizeof(*cmd)) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ /* If no connection handle exit with error */
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ /* If already pending exit with error */
+ if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG)) {
+ return BLE_ERR_CMD_DISALLOWED;
+ }
+
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG, NULL);
+
+ return BLE_ERR_SUCCESS;
}
int
ble_ll_cs_hci_wr_cached_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen,
uint8_t *rspbuf, uint8_t *rsplen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_wr_cached_rem_supp_cap_cp *cmd = (const void
*)cmdbuf;
+ struct ble_hci_le_cs_wr_cached_rem_supp_cap_rp *rsp = (void *)rspbuf;
+ struct ble_ll_cs_supp_cap *cap;
+ struct ble_ll_conn_sm *connsm;
+ uint16_t subfeatures;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ cap = &connsm->cssm->remote_cap;
+
+ cap->max_number_of_configs = cmd->num_config_supported;
+ cap->max_number_of_procedures =
le16toh(cmd->max_consecutive_procedures_supported);
+ cap->number_of_antennas = cmd->num_antennas_supported;
+ cap->max_number_of_antenna_paths = cmd->max_antenna_paths_supported;
+ cap->roles_supported = cmd->roles_supported;
+ cap->mode_types = cmd->optional_modes_supported;
+ cap->rtt_capability = cmd->rtt_capability;
+ cap->rtt_aa_only_n = cmd->rtt_aa_only_n;
+ cap->rtt_sounding_n = cmd->rtt_sounding_n;
+ cap->rtt_random_sequence_n = cmd->rtt_random_payload_n;
+ cap->nadm_sounding_capability =
le16toh(cmd->optional_nadm_sounding_capability);
+ cap->nadm_random_sequence_capability =
le16toh(cmd->optional_nadm_random_capability);
+ cap->cs_sync_phy_capability = cmd->optional_cs_sync_phys_supported;
+
+ subfeatures = le16toh(cmd->optional_subfeatures_supported);
+ cap->no_fae = (subfeatures >> 1) & 1;
+ cap->channel_selection = (subfeatures >> 2) & 1;
+ cap->sounding_pct_estimate = (subfeatures >> 3) & 1;
+
+ cap->t_ip1_capability = le16toh(cmd->optional_t_ip1_times_supported);
+ cap->t_ip2_capability = le16toh(cmd->optional_t_ip2_times_supported);
+ cap->t_fcs_capability = le16toh(cmd->optional_t_fcs_times_supported);
+ cap->t_pm_capability = le16toh(cmd->optional_t_pm_times_supported);
+ cap->t_sw = cmd->t_sw_time_supported;
+ cap->tx_snr_capablity = cmd->optional_tx_snr_capability;
+
+ rsp->conn_handle = cmd->conn_handle;
+ *rsplen = sizeof(*rsp);
+
+ return BLE_ERR_SUCCESS;
+}
+
+static int
+rej_ext_ind_make(uint8_t rej_opcode, uint8_t err, uint8_t *ctrdata)
+{
+ ctrdata[0] = rej_opcode;
+ ctrdata[1] = err;
+
+ return BLE_LL_CTRL_REJECT_IND_EXT;
+}
+
+int
+ble_ll_cs_rx_security_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+ uint8_t *rspbuf)
+{
+ uint8_t *iv = connsm->cssm->drbg_ctx.iv;
+ uint8_t *in = connsm->cssm->drbg_ctx.in;
+ uint8_t *pv = connsm->cssm->drbg_ctx.pv;
+
+ if (!connsm->flags.encrypted) {
+ return rej_ext_ind_make(BLE_LL_CTRL_CS_SEC_REQ,
Review Comment:
ble_ll_ctrl_rej_ext_ind_make + return
##########
nimble/controller/pkg.yml:
##########
@@ -42,7 +42,14 @@ pkg.deps:
- nimble
- nimble/transport
+pkg.deps.TEST:
+ - "@apache-mynewt-core/crypto/mbedtls"
+
pkg.init:
ble_ll_init:
- $before:ble_transport_hs_init
- $before:ble_transport_ll_init
+
+pkg.cflags.TEST:
+ - -Irepos/mbedtls/include
+ - -Irepos/include/mbedtls
Review Comment:
includes should be added automatically from pkg dep
--
This is an automated message from the Apache Git Service.
To respond to the message, please log on to GitHub and use the
URL above to go to the specific comment.
To unsubscribe, e-mail: commits-unsubscr...@mynewt.apache.org
For queries about this service, please contact Infrastructure at:
us...@infra.apache.org
