mkasenberg commented on code in PR #1765:
URL: https://github.com/apache/mynewt-nimble/pull/1765#discussion_r1595471438
##########
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:
Good point, there wasn't ;)
--
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]
