sjanc commented on code in PR #1765:
URL: https://github.com/apache/mynewt-nimble/pull/1765#discussion_r1594083001
##########
nimble/controller/src/ble_ll_cs.c:
##########
@@ -26,75 +26,956 @@
#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 */
+ return;
+ }
+
+ /* Save Peripheral's vector */
+ memcpy(drbg_ctx->iv + 8, dptr, 8);
+ memcpy(drbg_ctx->in + 4, dptr + 8, 4);
+ memcpy(drbg_ctx->pv + 8, dptr + 12, 8);
+
+ rc = ble_ll_cs_drbg_init(drbg_ctx);
+
+ /* Stop the control procedure and send an event to the host */
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_SEC_START);
+ ble_ll_cs_ev_sec_enable_complete(connsm, rc ? BLE_ERR_INV_LMP_LL_PARM :
+ BLE_ERR_SUCCESS);
+}
+
+void
+ble_ll_cs_security_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ uint8_t *iv = connsm->cssm->drbg_ctx.iv;
+ uint8_t *in = connsm->cssm->drbg_ctx.in;
+ uint8_t *pv = connsm->cssm->drbg_ctx.pv;
+
+ /* Generate Central's vector */
+ ble_ll_rand_data_get(iv, 8);
+ ble_ll_rand_data_get(in, 4);
+ ble_ll_rand_data_get(pv, 8);
+
+ memcpy(dptr, iv, 8);
+ memcpy(dptr + 8, in, 4);
+ memcpy(dptr + 12, pv, 8);
}
int
ble_ll_cs_hci_sec_enable(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_sec_enable_cp *cmd = (const void *)cmdbuf;
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ if (!connsm->flags.encrypted) {
+ return BLE_ERR_INSUFFICIENT_SEC;
+ }
+
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_SEC_START, NULL);
+
+ return BLE_ERR_SUCCESS;
}
int
ble_ll_cs_hci_set_def_settings(const uint8_t *cmdbuf, uint8_t cmdlen,
uint8_t *rspbuf, uint8_t *rsplen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_set_def_settings_cp *cmd = (const void *)cmdbuf;
+ struct ble_hci_le_cs_set_def_settings_rp *rsp = (void *)rspbuf;
+ const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+ struct ble_ll_conn_sm *connsm;
+ struct ble_ll_cs_sm *cssm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ cssm = connsm->cssm;
+
+ /* Check if a disabled role is used in CS configs */
+ for (int i = 0; i < ARRAY_SIZE(cssm->config); i++) {
Review Comment:
nitpick: we typically declare variable at top of the function, also could be
unsigned to avoid signed-unsigned comparison
##########
nimble/controller/src/ble_ll_cs.c:
##########
@@ -26,75 +26,956 @@
#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 */
+ return;
+ }
+
+ /* Save Peripheral's vector */
+ memcpy(drbg_ctx->iv + 8, dptr, 8);
+ memcpy(drbg_ctx->in + 4, dptr + 8, 4);
+ memcpy(drbg_ctx->pv + 8, dptr + 12, 8);
+
+ rc = ble_ll_cs_drbg_init(drbg_ctx);
+
+ /* Stop the control procedure and send an event to the host */
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_SEC_START);
+ ble_ll_cs_ev_sec_enable_complete(connsm, rc ? BLE_ERR_INV_LMP_LL_PARM :
+ BLE_ERR_SUCCESS);
+}
+
+void
+ble_ll_cs_security_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ uint8_t *iv = connsm->cssm->drbg_ctx.iv;
+ uint8_t *in = connsm->cssm->drbg_ctx.in;
+ uint8_t *pv = connsm->cssm->drbg_ctx.pv;
+
+ /* Generate Central's vector */
+ ble_ll_rand_data_get(iv, 8);
+ ble_ll_rand_data_get(in, 4);
+ ble_ll_rand_data_get(pv, 8);
+
+ memcpy(dptr, iv, 8);
+ memcpy(dptr + 8, in, 4);
+ memcpy(dptr + 12, pv, 8);
}
int
ble_ll_cs_hci_sec_enable(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_sec_enable_cp *cmd = (const void *)cmdbuf;
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ if (!connsm->flags.encrypted) {
+ return BLE_ERR_INSUFFICIENT_SEC;
+ }
+
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_SEC_START, NULL);
+
+ return BLE_ERR_SUCCESS;
}
int
ble_ll_cs_hci_set_def_settings(const uint8_t *cmdbuf, uint8_t cmdlen,
uint8_t *rspbuf, uint8_t *rsplen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_set_def_settings_cp *cmd = (const void *)cmdbuf;
+ struct ble_hci_le_cs_set_def_settings_rp *rsp = (void *)rspbuf;
+ const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+ struct ble_ll_conn_sm *connsm;
+ struct ble_ll_cs_sm *cssm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ cssm = connsm->cssm;
+
+ /* Check if a disabled role is used in CS configs */
+ for (int i = 0; i < ARRAY_SIZE(cssm->config); i++) {
+ struct ble_ll_cs_config *conf = &cssm->config[i];
+
+ if (conf->config_enabled && (1 << conf->role) & ~cmd->role_enable) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+ }
+
+ if ((cmd->role_enable & ~cap->roles_supported) != 0 ||
+ (cap->number_of_antennas < cmd->cs_sync_antenna_selection &&
+ cmd->cs_sync_antenna_selection < 0xFE)) {
+ /* Unsupported role or antenna selection used */
+ return BLE_ERR_UNSUPPORTED;
+ }
+
+ cssm->roles_enabled = cmd->role_enable;
+ cssm->cs_sync_antenna_selection = cmd->cs_sync_antenna_selection;
+
+ /* Allowed Transmit_Power_Level range: -127 to +20,
+ * (Complement system + special meaning for 0x7E and 0x7F)
+ */
+ if (!(IN_RANGE(cmd->max_tx_power, 0x00, 0x14) ||
+ IN_RANGE(cmd->max_tx_power, 0x7E, 0xFF))) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ if (cmd->max_tx_power == 0x7E) {
+ /* TODO: Set transmitter to minimum transmit power level
+ * supported by the board.
+ */
+ cssm->max_tx_power = 0x80;
+ } else if (cmd->max_tx_power == 0x7F) {
+ /* TODO: Set transmitter to maximum transmit power level
+ * supported by the board.
+ */
+ cssm->max_tx_power = 0x14;
+ } else {
+ /* TODO: Set transmitter to the nearest transmit power level
+ * supported by the board.
+ */
+ cssm->max_tx_power = cmd->max_tx_power;
+ }
+
+ rsp->conn_handle = cmd->conn_handle;
+ *rsplen = sizeof(*rsp);
+
+ return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_cs_rx_fae_req(struct ble_ll_conn_sm *connsm, uint8_t *rspbuf)
+{
+ memcpy(rspbuf, connsm->cssm->local_fae_table, 72);
+
+ return BLE_LL_CTRL_CS_FAE_RSP;
+}
+
+static void
+ble_ll_cs_ev_rd_rem_fae_complete(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+ struct ble_hci_ev_le_subev_cs_rd_rem_fae_complete *ev;
+ struct ble_hci_ev *hci_ev;
+
+ if (ble_ll_hci_is_le_event_enabled(
+ BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_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_FAE_COMPLETE;
+ ev->status = status;
+ ev->conn_handle = htole16(connsm->conn_handle);
+ memcpy(ev->remote_fae_table, connsm->cssm->remote_fae_table, 72);
+
+ ble_ll_hci_event_send(hci_ev);
+ }
+ }
+}
+
+void
+ble_ll_cs_rx_fae_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ)) {
+ /* Should never happen */
+ return;
+ }
+
+ memcpy(connsm->cssm->remote_fae_table, dptr, 72);
+
+ /* Stop the control procedure and send an event to the host */
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ);
+ ble_ll_cs_ev_rd_rem_fae_complete(connsm, BLE_ERR_SUCCESS);
}
int
ble_ll_cs_hci_rd_rem_fae(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_rd_rem_fae_cp *cmd = (const void *)cmdbuf;
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ, NULL);
+
+ return BLE_ERR_SUCCESS;
}
int
ble_ll_cs_hci_wr_cached_rem_fae(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_fae_cp *cmd = (const void
*)cmdbuf;
+ struct ble_hci_le_cs_wr_cached_rem_fae_rp *rsp = (void *)rspbuf;
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ memcpy(connsm->cssm->remote_fae_table, cmd->remote_fae_table, 72);
+
+ rsp->conn_handle = cmd->conn_handle;
+ *rsplen = sizeof(*rsp);
+
+ return BLE_ERR_SUCCESS;
+}
+
+void
+ble_ll_cs_config_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ uint8_t config_id = connsm->cssm->config_req_id;
+ uint8_t action = connsm->cssm->config_req_action;
+ const struct ble_ll_cs_config *conf = &connsm->cssm->config[config_id];
+
+ *dptr = config_id | action << 6;
+
+ if (action == 0x00) {
+ /* Removing the config, all remaining fields are RFU. */
+ memset(dptr + 1, 0, 26);
+
+ return;
+ }
+
+ memcpy(dptr + 1, conf->chan_map, 10);
+ dptr[11] = conf->chan_map_repetition;
+ dptr[12] = conf->main_mode;
+ dptr[13] = conf->sub_mode;
+ dptr[14] = conf->main_mode_min_steps;
+ dptr[15] = conf->main_mode_max_steps;
+ dptr[16] = conf->main_mode_repetition;
+ dptr[17] = conf->mode_0_steps;
+ dptr[18] = conf->cs_sync_phy;
+ dptr[19] = conf->rtt_type |
+ conf->role << 4;
+ dptr[20] = conf->chan_sel |
+ conf->ch3cshape << 4;
+ dptr[21] = conf->ch3cjump;
+ dptr[22] = conf->t_ip1;
+ dptr[23] = conf->t_ip2;
+ dptr[24] = conf->t_fcs;
+ dptr[25] = conf->t_pm;
+ /* RFU octet */
+ dptr[26] = 0x00;
+}
+
+int
+ble_ll_cs_rx_config_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+ uint8_t *rspbuf)
+{
+ struct ble_ll_cs_config *conf;
+ uint8_t config_id = *dptr & 0b00111111;
+ uint8_t action = (*dptr & 0b11000000) >> 6;
+
+ if (config_id >= BLE_LL_CS_CONFIG_MAX_NUM) {
+ return rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ,
+ BLE_ERR_INV_LMP_LL_PARM, rspbuf);
+ }
+
+ conf = &connsm->cssm->config[config_id];
+ memset(conf, 0, sizeof(*conf));
+
+ /* Respond with LL_CS_CONFIG_RSP PDU */
+ *rspbuf = config_id | action << 6;
+
+ if (action == 0x00) {
+ /* CS configuration removed. */
+ return BLE_LL_CTRL_CS_CONFIG_RSP;
+ }
+
+ memcpy(conf->chan_map, dptr + 1, 10);
+ conf->chan_map_repetition = dptr[11];
+ conf->main_mode = dptr[12];
+ conf->sub_mode = dptr[13];
+ conf->main_mode_min_steps = dptr[14];
+ conf->main_mode_max_steps = dptr[15];
+ conf->main_mode_repetition = dptr[16];
+ conf->mode_0_steps = dptr[17];
+ conf->cs_sync_phy = dptr[18];
+ conf->rtt_type = dptr[19] & 0b00001111;
+ conf->role = (~dptr[19] >> 4) & 0b00000001;
+ conf->chan_sel = (dptr[20] & 0b00001111);
+ conf->ch3cshape = (dptr[20] & 0b11110000) >> 4;
+ conf->ch3cjump = dptr[21];
+ conf->t_ip1 = dptr[22];
+ conf->t_ip2 = dptr[23];
+ conf->t_fcs = dptr[24];
+ conf->t_pm = dptr[25];
+ conf->config_enabled = true;
+
+ return BLE_LL_CTRL_CS_CONFIG_RSP;
+}
+
+static void
+ble_ll_cs_ev_config_complete(struct ble_ll_conn_sm *connsm, uint8_t config_id,
+ uint8_t action, uint8_t status)
+{
+ struct ble_hci_ev_le_subev_cs_config_complete *ev;
+ const struct ble_ll_cs_config *conf;
+ struct ble_hci_ev *hci_ev;
+
+ if (ble_ll_hci_is_le_event_enabled(
+ BLE_HCI_LE_SUBEV_CS_CONFIG_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;
+
+ memset(ev, 0, sizeof(*ev));
+ ev->subev_code = BLE_HCI_LE_SUBEV_CS_CONFIG_COMPLETE;
+ ev->status = status;
+ ev->conn_handle = htole16(connsm->conn_handle);
+ ev->config_id = config_id;
+ ev->action = action;
+
+ if (action == 0x00) {
+ /* CS configuration removed. */
+ return;
+ }
+
+ conf = &connsm->cssm->config[config_id];
+ ev->main_mode_type = conf->main_mode;
+ ev->sub_mode_type = conf->sub_mode;
+ ev->min_main_mode_steps = conf->main_mode_min_steps;
+ ev->max_main_mode_steps = conf->main_mode_max_steps;
+ ev->main_mode_repetition = conf->main_mode_repetition;
+ ev->mode_0_steps = conf->mode_0_steps;
+ ev->role = conf->role;
+ ev->rtt_type = conf->rtt_type;
+ ev->cs_sync_phy = conf->cs_sync_phy;
+ memcpy(ev->channel_map, conf->chan_map, 10);
+ ev->channel_map_repetition = conf->chan_map_repetition;
+ ev->channel_selection_type = conf->chan_sel;
+ ev->ch3c_shape = conf->ch3cshape;
+ ev->ch3c_jump = conf->ch3cjump;
+ ev->reserved = 0x00;
+ ev->t_ip1_time = conf->t_ip1;
+ ev->t_ip2_time = conf->t_ip2;
+ ev->t_fcs_time = conf->t_fcs;
+ ev->t_pm_time = conf->t_pm;
+
+ ble_ll_hci_event_send(hci_ev);
+ }
+ }
+}
+
+void
+ble_ll_cs_rx_config_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ uint8_t config_id = *dptr & 0b00111111;
+ uint8_t action = (*dptr & 0b11000000) >> 6;
+
+ if (config_id != connsm->cssm->config_req_id ||
+ !IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF)) {
+ return;
+ }
+
+ /* Stop the control procedure and send an event to the host */
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CONF);
+ ble_ll_cs_ev_config_complete(connsm, config_id, action, BLE_ERR_SUCCESS);
+}
+
+static int
+ble_ll_cs_select_capability(const uint8_t *capability_values, uint8_t len,
+ uint8_t *out_value, uint16_t local_capability,
+ uint16_t remote_capability)
+{
+ uint16_t common_capability = local_capability & remote_capability;
+ uint8_t i;
+
+ for (i = 0; i < len; i++) {
+ if ((common_capability >> i) & 1) {
+ *out_value = capability_values[i];
+ return 0;
+ }
+ }
+
+ return 1;
}
int
ble_ll_cs_hci_create_config(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_create_config_cp *cmd = (const void *)cmdbuf;
+ struct ble_ll_conn_sm *connsm;
+ struct ble_ll_cs_config *conf;
+ struct ble_ll_cs_sm *cssm;
+
+ if (cmdlen != sizeof(*cmd) || cmd->config_id >= BLE_LL_CS_CONFIG_MAX_NUM) {
+ 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;
+ }
+
+ cssm = connsm->cssm;
+
+ /* If already pending exit with error */
+ if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF)) {
+ return BLE_ERR_CMD_DISALLOWED;
+ }
+
+ /* Configure CS config locally */
+ conf = &cssm->config[cmd->config_id];
+ conf->config_enabled = true;
+ conf->main_mode = cmd->main_mode_type;
+ conf->sub_mode = cmd->sub_mode_type;
+ conf->main_mode_min_steps = cmd->min_main_mode_steps;
+ conf->main_mode_max_steps = cmd->max_main_mode_steps;
+ conf->main_mode_repetition = cmd->main_mode_repetition;
+ conf->mode_0_steps = cmd->mode_0_steps;
+ conf->role = cmd->role;
+ conf->rtt_type = cmd->rtt_type;
+ conf->cs_sync_phy = cmd->cs_sync_phy;
+ memcpy(conf->chan_map, cmd->channel_map, 10);
+ conf->chan_map_repetition = cmd->channel_map_repetition;
+ conf->chan_sel = cmd->channel_selection_type;
+ conf->ch3cshape = cmd->ch3c_shape;
+ conf->ch3cjump = cmd->ch3c_jump;
+
+ if (ble_ll_cs_select_capability(t_ip1, ARRAY_SIZE(t_ip1), &conf->t_ip1,
+ cssm->remote_cap.t_ip1_capability,
+ g_ble_ll_cs_local_cap.t_ip1_capability)) {
+ memset(conf, 0, sizeof(*conf));
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ if (ble_ll_cs_select_capability(t_ip2, ARRAY_SIZE(t_ip2), &conf->t_ip2,
+ cssm->remote_cap.t_ip2_capability,
+ g_ble_ll_cs_local_cap.t_ip2_capability)) {
+ memset(conf, 0, sizeof(*conf));
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ if (ble_ll_cs_select_capability(t_fcs, ARRAY_SIZE(t_fcs), &conf->t_fcs,
+ cssm->remote_cap.t_fcs_capability,
+ g_ble_ll_cs_local_cap.t_fcs_capability)) {
+ memset(conf, 0, sizeof(*conf));
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ if (ble_ll_cs_select_capability(t_pm, ARRAY_SIZE(t_pm), &conf->t_pm,
+ cssm->remote_cap.t_pm_capability,
+ g_ble_ll_cs_local_cap.t_pm_capability)) {
+ memset(conf, 0, sizeof(*conf));
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ if (cmd->create_context == 0x01) {
+ /* Configure the CS config in the remote controller */
+ cssm->config_req_id = cmd->config_id;
+ cssm->config_req_action = 0x01;
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_CONF, NULL);
+ } else {
+ ble_ll_cs_ev_config_complete(connsm, cmd->config_id, 0x01,
BLE_ERR_SUCCESS);
+ }
+
+ return BLE_ERR_SUCCESS;
}
int
ble_ll_cs_hci_remove_config(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_remove_config_cp *cmd = (const void *)cmdbuf;
+ struct ble_ll_conn_sm *connsm;
+ struct ble_ll_cs_sm *cssm;
+ struct ble_ll_cs_config *conf;
+
+ if (cmdlen != sizeof(*cmd) || cmd->config_id >= BLE_LL_CS_CONFIG_MAX_NUM) {
+ 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;
+ }
+
+ cssm = connsm->cssm;
+
+ /* If already pending exit with error */
+ if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF)) {
+ return BLE_ERR_CMD_DISALLOWED;
+ }
+
+ /* Remove the CS config locally */
+ conf = &cssm->config[cmd->config_id];
+ memset(conf, 0, sizeof(*conf));
+
+ /* Configure the CS config in the remote controller */
+ cssm->config_req_id = cmd->config_id;
+ cssm->config_req_action = 0x00;
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_CONF, NULL);
+
+ return BLE_ERR_SUCCESS;
+}
+
+static int
+ble_ll_cs_proc_set_chan_class(const uint8_t *channel_classification)
+{
+ uint8_t i, j, next_id, byte;
+
+ /* TODO:
+ * 1. The interval between two successive commands sent shall be at least
1 second.
+ * Otherwise, the Controller shall return the error code Command
Disallowed (0x0C).
+ *
+ * 2. Combine the Host chan_class with local chan_class capabilities?
+ */
+
+ if (channel_classification[0] & 0b00000011 ||
+ channel_classification[2] & 0b10000000 ||
+ channel_classification[3] & 0b00000011 ||
+ channel_classification[9] & 0b11100000) {
+ /* Channels 0, 1, 23, 24, 25, 77, 78, and the bit 79 (non-channel)
+ * are RFU. At least 15 channels shall be enabled.
+ */
+ return -1;
+ }
+
+ for (i = 0, j = 0; i < ARRAY_SIZE(g_ble_ll_cs_chan_class); ++i) {
+ byte = channel_classification[i];
+ next_id = i * 8;
+
+ while (byte) {
+ if (byte & 1) {
+ g_ble_ll_cs_chan_indices[j++] = next_id;
+ }
+ ++next_id;
+ byte >>= 1;
+ }
+ }
+
+ g_ble_ll_cs_chan_count = j;
+ if (g_ble_ll_cs_chan_count < 15) {
+ return -1;
+ }
+
+ memcpy(g_ble_ll_cs_chan_class, channel_classification,
+ sizeof(g_ble_ll_cs_chan_class));
+
+ return 0;
}
int
ble_ll_cs_hci_set_chan_class(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ int rc;
+ const struct ble_hci_le_cs_set_chan_class_cp *cmd = (const void *)cmdbuf;
+
+ rc = ble_ll_cs_proc_set_chan_class(cmd->channel_classification);
+ if (rc) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ return BLE_ERR_SUCCESS;
}
int
ble_ll_cs_hci_set_proc_params(const uint8_t *cmdbuf, uint8_t cmdlen,
uint8_t *rspbuf, uint8_t *rsplen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_set_proc_params_cp *cmd = (const void *)cmdbuf;
+ struct ble_hci_le_cs_set_proc_params_rp *rsp = (void *)rspbuf;
+ struct ble_ll_conn_sm *connsm;
+ struct ble_ll_cs_config *conf;
+ struct ble_ll_cs_pref_proc_params *params;
+
+ if (cmdlen != sizeof(*cmd) || cmd->config_id >= BLE_LL_CS_CONFIG_MAX_NUM) {
+ 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;
+ }
+
+ conf = &connsm->cssm->config[cmd->config_id];
Review Comment:
config_id should be validated before being used for indexing
##########
nimble/controller/src/ble_ll_cs.c:
##########
@@ -26,75 +26,956 @@
#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 */
+ return;
+ }
+
+ /* Save Peripheral's vector */
+ memcpy(drbg_ctx->iv + 8, dptr, 8);
+ memcpy(drbg_ctx->in + 4, dptr + 8, 4);
+ memcpy(drbg_ctx->pv + 8, dptr + 12, 8);
+
+ rc = ble_ll_cs_drbg_init(drbg_ctx);
+
+ /* Stop the control procedure and send an event to the host */
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_SEC_START);
+ ble_ll_cs_ev_sec_enable_complete(connsm, rc ? BLE_ERR_INV_LMP_LL_PARM :
+ BLE_ERR_SUCCESS);
+}
+
+void
+ble_ll_cs_security_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ uint8_t *iv = connsm->cssm->drbg_ctx.iv;
+ uint8_t *in = connsm->cssm->drbg_ctx.in;
+ uint8_t *pv = connsm->cssm->drbg_ctx.pv;
+
+ /* Generate Central's vector */
+ ble_ll_rand_data_get(iv, 8);
+ ble_ll_rand_data_get(in, 4);
+ ble_ll_rand_data_get(pv, 8);
+
+ memcpy(dptr, iv, 8);
+ memcpy(dptr + 8, in, 4);
+ memcpy(dptr + 12, pv, 8);
}
int
ble_ll_cs_hci_sec_enable(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_sec_enable_cp *cmd = (const void *)cmdbuf;
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ if (!connsm->flags.encrypted) {
+ return BLE_ERR_INSUFFICIENT_SEC;
+ }
+
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_SEC_START, NULL);
+
+ return BLE_ERR_SUCCESS;
}
int
ble_ll_cs_hci_set_def_settings(const uint8_t *cmdbuf, uint8_t cmdlen,
uint8_t *rspbuf, uint8_t *rsplen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_set_def_settings_cp *cmd = (const void *)cmdbuf;
+ struct ble_hci_le_cs_set_def_settings_rp *rsp = (void *)rspbuf;
+ const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+ struct ble_ll_conn_sm *connsm;
+ struct ble_ll_cs_sm *cssm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ cssm = connsm->cssm;
+
+ /* Check if a disabled role is used in CS configs */
+ for (int i = 0; i < ARRAY_SIZE(cssm->config); i++) {
+ struct ble_ll_cs_config *conf = &cssm->config[i];
+
+ if (conf->config_enabled && (1 << conf->role) & ~cmd->role_enable) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+ }
+
+ if ((cmd->role_enable & ~cap->roles_supported) != 0 ||
+ (cap->number_of_antennas < cmd->cs_sync_antenna_selection &&
+ cmd->cs_sync_antenna_selection < 0xFE)) {
+ /* Unsupported role or antenna selection used */
+ return BLE_ERR_UNSUPPORTED;
+ }
+
+ cssm->roles_enabled = cmd->role_enable;
+ cssm->cs_sync_antenna_selection = cmd->cs_sync_antenna_selection;
+
+ /* Allowed Transmit_Power_Level range: -127 to +20,
+ * (Complement system + special meaning for 0x7E and 0x7F)
+ */
+ if (!(IN_RANGE(cmd->max_tx_power, 0x00, 0x14) ||
+ IN_RANGE(cmd->max_tx_power, 0x7E, 0xFF))) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ if (cmd->max_tx_power == 0x7E) {
+ /* TODO: Set transmitter to minimum transmit power level
+ * supported by the board.
+ */
+ cssm->max_tx_power = 0x80;
+ } else if (cmd->max_tx_power == 0x7F) {
+ /* TODO: Set transmitter to maximum transmit power level
+ * supported by the board.
+ */
+ cssm->max_tx_power = 0x14;
+ } else {
+ /* TODO: Set transmitter to the nearest transmit power level
+ * supported by the board.
+ */
+ cssm->max_tx_power = cmd->max_tx_power;
+ }
+
+ rsp->conn_handle = cmd->conn_handle;
+ *rsplen = sizeof(*rsp);
+
+ return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_cs_rx_fae_req(struct ble_ll_conn_sm *connsm, uint8_t *rspbuf)
+{
+ memcpy(rspbuf, connsm->cssm->local_fae_table, 72);
+
+ return BLE_LL_CTRL_CS_FAE_RSP;
+}
+
+static void
+ble_ll_cs_ev_rd_rem_fae_complete(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+ struct ble_hci_ev_le_subev_cs_rd_rem_fae_complete *ev;
+ struct ble_hci_ev *hci_ev;
+
+ if (ble_ll_hci_is_le_event_enabled(
+ BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_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_FAE_COMPLETE;
+ ev->status = status;
+ ev->conn_handle = htole16(connsm->conn_handle);
+ memcpy(ev->remote_fae_table, connsm->cssm->remote_fae_table, 72);
+
+ ble_ll_hci_event_send(hci_ev);
+ }
+ }
+}
+
+void
+ble_ll_cs_rx_fae_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ)) {
+ /* Should never happen */
+ return;
+ }
+
+ memcpy(connsm->cssm->remote_fae_table, dptr, 72);
+
+ /* Stop the control procedure and send an event to the host */
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ);
+ ble_ll_cs_ev_rd_rem_fae_complete(connsm, BLE_ERR_SUCCESS);
}
int
ble_ll_cs_hci_rd_rem_fae(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_rd_rem_fae_cp *cmd = (const void *)cmdbuf;
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_FAE_REQ, NULL);
+
+ return BLE_ERR_SUCCESS;
}
int
ble_ll_cs_hci_wr_cached_rem_fae(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_fae_cp *cmd = (const void
*)cmdbuf;
+ struct ble_hci_le_cs_wr_cached_rem_fae_rp *rsp = (void *)rspbuf;
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ memcpy(connsm->cssm->remote_fae_table, cmd->remote_fae_table, 72);
+
+ rsp->conn_handle = cmd->conn_handle;
+ *rsplen = sizeof(*rsp);
+
+ return BLE_ERR_SUCCESS;
+}
+
+void
+ble_ll_cs_config_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ uint8_t config_id = connsm->cssm->config_req_id;
+ uint8_t action = connsm->cssm->config_req_action;
+ const struct ble_ll_cs_config *conf = &connsm->cssm->config[config_id];
Review Comment:
is config_id already validated here?
##########
nimble/controller/src/ble_ll_cs_drbg.c:
##########
@@ -0,0 +1,686 @@
+/*
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements. See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership. The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied. See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include <syscfg/syscfg.h>
+#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
+#include <stdint.h>
+#include <assert.h>
+#include <os/endian.h>
+#include "nimble/ble.h"
+#include "mbedtls/aes.h"
+#include "ble_ll_cs_drbg_priv.h"
+
+static const uint8_t rtt_seq_len[] = {0, 4, 12, 4, 8, 12, 16};
+
+/**
+ * Security function e generates 128-bit encrypted_data from a 128-bit key
+ * and 128-bit data using the AES-128-bit block cypher.
+ */
+int
+ble_ll_cs_drbg_e(const uint8_t *key, const uint8_t *data, uint8_t *out_data)
+{
+ struct ble_encryption_block ecb;
+ int rc = 0;
+
+ /* The cryptographic function uses the leftmost to rightmost
+ * representation (MSO to LSO).
+ */
+ swap_buf(ecb.key, key, BLE_ENC_BLOCK_SIZE);
+ swap_buf(ecb.plain_text, data, BLE_ENC_BLOCK_SIZE);
+
+#if BABBLESIM || MYNEWT_VAL(SELFTEST)
+ /* Use software to encrypt the data */
+ mbedtls_aes_context aes_ctx;
+ mbedtls_aes_init(&aes_ctx);
+ mbedtls_aes_setkey_enc(&aes_ctx, ecb.key, 16 * 8);
+ rc = mbedtls_aes_crypt_ecb(&aes_ctx, MBEDTLS_AES_ENCRYPT, ecb.plain_text,
ecb.cipher_text);
+ mbedtls_aes_free(&aes_ctx);
+#else
+ /* Use hardware to encrypt the data */
+ rc = ble_hw_encrypt_block(&ecb);
+#endif
+
+ if (!rc) {
+ swap_buf(out_data, ecb.cipher_text, BLE_ENC_BLOCK_SIZE);
+ rc = 0;
Review Comment:
rc is already 0 here
##########
nimble/controller/src/ble_ll_cs.c:
##########
@@ -26,75 +26,956 @@
#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 */
+ return;
+ }
+
+ /* Save Peripheral's vector */
+ memcpy(drbg_ctx->iv + 8, dptr, 8);
+ memcpy(drbg_ctx->in + 4, dptr + 8, 4);
+ memcpy(drbg_ctx->pv + 8, dptr + 12, 8);
+
+ rc = ble_ll_cs_drbg_init(drbg_ctx);
+
+ /* Stop the control procedure and send an event to the host */
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_SEC_START);
+ ble_ll_cs_ev_sec_enable_complete(connsm, rc ? BLE_ERR_INV_LMP_LL_PARM :
+ BLE_ERR_SUCCESS);
+}
+
+void
+ble_ll_cs_security_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ uint8_t *iv = connsm->cssm->drbg_ctx.iv;
+ uint8_t *in = connsm->cssm->drbg_ctx.in;
+ uint8_t *pv = connsm->cssm->drbg_ctx.pv;
+
+ /* Generate Central's vector */
+ ble_ll_rand_data_get(iv, 8);
+ ble_ll_rand_data_get(in, 4);
+ ble_ll_rand_data_get(pv, 8);
+
+ memcpy(dptr, iv, 8);
+ memcpy(dptr + 8, in, 4);
+ memcpy(dptr + 12, pv, 8);
}
int
ble_ll_cs_hci_sec_enable(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_sec_enable_cp *cmd = (const void *)cmdbuf;
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ if (!connsm->flags.encrypted) {
+ return BLE_ERR_INSUFFICIENT_SEC;
+ }
+
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_SEC_START, NULL);
+
+ return BLE_ERR_SUCCESS;
}
int
ble_ll_cs_hci_set_def_settings(const uint8_t *cmdbuf, uint8_t cmdlen,
uint8_t *rspbuf, uint8_t *rsplen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_set_def_settings_cp *cmd = (const void *)cmdbuf;
+ struct ble_hci_le_cs_set_def_settings_rp *rsp = (void *)rspbuf;
+ const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+ struct ble_ll_conn_sm *connsm;
+ struct ble_ll_cs_sm *cssm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ cssm = connsm->cssm;
+
+ /* Check if a disabled role is used in CS configs */
+ for (int i = 0; i < ARRAY_SIZE(cssm->config); i++) {
+ struct ble_ll_cs_config *conf = &cssm->config[i];
Review Comment:
same here
##########
nimble/controller/src/ble_ll_cs.c:
##########
@@ -26,75 +26,956 @@
#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 */
+ return;
+ }
+
+ /* Save Peripheral's vector */
+ memcpy(drbg_ctx->iv + 8, dptr, 8);
+ memcpy(drbg_ctx->in + 4, dptr + 8, 4);
+ memcpy(drbg_ctx->pv + 8, dptr + 12, 8);
+
+ rc = ble_ll_cs_drbg_init(drbg_ctx);
+
+ /* Stop the control procedure and send an event to the host */
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_SEC_START);
+ ble_ll_cs_ev_sec_enable_complete(connsm, rc ? BLE_ERR_INV_LMP_LL_PARM :
+ BLE_ERR_SUCCESS);
+}
+
+void
+ble_ll_cs_security_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ uint8_t *iv = connsm->cssm->drbg_ctx.iv;
+ uint8_t *in = connsm->cssm->drbg_ctx.in;
+ uint8_t *pv = connsm->cssm->drbg_ctx.pv;
+
+ /* Generate Central's vector */
+ ble_ll_rand_data_get(iv, 8);
+ ble_ll_rand_data_get(in, 4);
+ ble_ll_rand_data_get(pv, 8);
+
+ memcpy(dptr, iv, 8);
+ memcpy(dptr + 8, in, 4);
+ memcpy(dptr + 12, pv, 8);
}
int
ble_ll_cs_hci_sec_enable(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_sec_enable_cp *cmd = (const void *)cmdbuf;
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ if (!connsm->flags.encrypted) {
+ return BLE_ERR_INSUFFICIENT_SEC;
+ }
+
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_SEC_START, NULL);
+
+ return BLE_ERR_SUCCESS;
}
int
ble_ll_cs_hci_set_def_settings(const uint8_t *cmdbuf, uint8_t cmdlen,
uint8_t *rspbuf, uint8_t *rsplen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_set_def_settings_cp *cmd = (const void *)cmdbuf;
+ struct ble_hci_le_cs_set_def_settings_rp *rsp = (void *)rspbuf;
+ const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+ struct ble_ll_conn_sm *connsm;
+ struct ble_ll_cs_sm *cssm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ cssm = connsm->cssm;
+
+ /* Check if a disabled role is used in CS configs */
+ for (int i = 0; i < ARRAY_SIZE(cssm->config); i++) {
+ struct ble_ll_cs_config *conf = &cssm->config[i];
+
+ if (conf->config_enabled && (1 << conf->role) & ~cmd->role_enable) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+ }
+
+ if ((cmd->role_enable & ~cap->roles_supported) != 0 ||
+ (cap->number_of_antennas < cmd->cs_sync_antenna_selection &&
+ cmd->cs_sync_antenna_selection < 0xFE)) {
+ /* Unsupported role or antenna selection used */
+ return BLE_ERR_UNSUPPORTED;
+ }
+
+ cssm->roles_enabled = cmd->role_enable;
+ cssm->cs_sync_antenna_selection = cmd->cs_sync_antenna_selection;
+
+ /* Allowed Transmit_Power_Level range: -127 to +20,
+ * (Complement system + special meaning for 0x7E and 0x7F)
+ */
+ if (!(IN_RANGE(cmd->max_tx_power, 0x00, 0x14) ||
+ IN_RANGE(cmd->max_tx_power, 0x7E, 0xFF))) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+
+ if (cmd->max_tx_power == 0x7E) {
+ /* TODO: Set transmitter to minimum transmit power level
+ * supported by the board.
+ */
+ cssm->max_tx_power = 0x80;
+ } else if (cmd->max_tx_power == 0x7F) {
+ /* TODO: Set transmitter to maximum transmit power level
+ * supported by the board.
+ */
+ cssm->max_tx_power = 0x14;
+ } else {
+ /* TODO: Set transmitter to the nearest transmit power level
+ * supported by the board.
+ */
+ cssm->max_tx_power = cmd->max_tx_power;
+ }
+
+ rsp->conn_handle = cmd->conn_handle;
+ *rsplen = sizeof(*rsp);
+
+ return BLE_ERR_SUCCESS;
+}
+
+int
+ble_ll_cs_rx_fae_req(struct ble_ll_conn_sm *connsm, uint8_t *rspbuf)
+{
+ memcpy(rspbuf, connsm->cssm->local_fae_table, 72);
Review Comment:
this is quite a lot, is it guaranteed that there is enough linear space in
underlying mbuf?
##########
nimble/controller/src/ble_ll_cs.c:
##########
@@ -26,75 +26,956 @@
#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 */
+ return;
+ }
+
+ /* Save Peripheral's vector */
+ memcpy(drbg_ctx->iv + 8, dptr, 8);
+ memcpy(drbg_ctx->in + 4, dptr + 8, 4);
+ memcpy(drbg_ctx->pv + 8, dptr + 12, 8);
+
+ rc = ble_ll_cs_drbg_init(drbg_ctx);
+
+ /* Stop the control procedure and send an event to the host */
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_SEC_START);
+ ble_ll_cs_ev_sec_enable_complete(connsm, rc ? BLE_ERR_INV_LMP_LL_PARM :
+ BLE_ERR_SUCCESS);
+}
+
+void
+ble_ll_cs_security_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ uint8_t *iv = connsm->cssm->drbg_ctx.iv;
+ uint8_t *in = connsm->cssm->drbg_ctx.in;
+ uint8_t *pv = connsm->cssm->drbg_ctx.pv;
+
+ /* Generate Central's vector */
+ ble_ll_rand_data_get(iv, 8);
+ ble_ll_rand_data_get(in, 4);
+ ble_ll_rand_data_get(pv, 8);
+
+ memcpy(dptr, iv, 8);
+ memcpy(dptr + 8, in, 4);
+ memcpy(dptr + 12, pv, 8);
}
int
ble_ll_cs_hci_sec_enable(const uint8_t *cmdbuf, uint8_t cmdlen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_sec_enable_cp *cmd = (const void *)cmdbuf;
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ if (!connsm->flags.encrypted) {
+ return BLE_ERR_INSUFFICIENT_SEC;
+ }
+
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_SEC_START, NULL);
+
+ return BLE_ERR_SUCCESS;
}
int
ble_ll_cs_hci_set_def_settings(const uint8_t *cmdbuf, uint8_t cmdlen,
uint8_t *rspbuf, uint8_t *rsplen)
{
- return BLE_ERR_UNSUPPORTED;
+ const struct ble_hci_le_cs_set_def_settings_cp *cmd = (const void *)cmdbuf;
+ struct ble_hci_le_cs_set_def_settings_rp *rsp = (void *)rspbuf;
+ const struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap;
+ struct ble_ll_conn_sm *connsm;
+ struct ble_ll_cs_sm *cssm;
+
+ connsm = ble_ll_conn_find_by_handle(le16toh(cmd->conn_handle));
+ if (!connsm) {
+ return BLE_ERR_UNK_CONN_ID;
+ }
+
+ cssm = connsm->cssm;
+
+ /* Check if a disabled role is used in CS configs */
+ for (int i = 0; i < ARRAY_SIZE(cssm->config); i++) {
+ struct ble_ll_cs_config *conf = &cssm->config[i];
+
+ if (conf->config_enabled && (1 << conf->role) & ~cmd->role_enable) {
+ return BLE_ERR_INV_HCI_CMD_PARMS;
+ }
+ }
+
+ if ((cmd->role_enable & ~cap->roles_supported) != 0 ||
+ (cap->number_of_antennas < cmd->cs_sync_antenna_selection &&
+ cmd->cs_sync_antenna_selection < 0xFE)) {
+ /* Unsupported role or antenna selection used */
+ return BLE_ERR_UNSUPPORTED;
+ }
+
+ cssm->roles_enabled = cmd->role_enable;
+ cssm->cs_sync_antenna_selection = cmd->cs_sync_antenna_selection;
Review Comment:
not sure if we are very consistent with this but maybe sm could be modified
only after all validation is done? ie to avoid modifying it and stil returning
error status
--
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: [email protected]
For queries about this service, please contact Infrastructure at:
[email protected]
