diff --git a/nimble/controller/include/controller/ble_ll.h b/nimble/controller/include/controller/ble_ll.h index ab75edcba3..1f8527f158 100644 --- a/nimble/controller/include/controller/ble_ll.h +++ b/nimble/controller/include/controller/ble_ll.h @@ -33,6 +33,10 @@ #include "hal/hal_timer.h" #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include "controller/ble_ll_cs.h" +#endif + #ifdef __cplusplus extern "C" { #endif @@ -243,6 +247,9 @@ extern STATS_SECT_DECL(ble_ll_stats) ble_ll_stats; #if MYNEWT_VAL(BLE_LL_ISO_BROADCASTER) #define BLE_LL_STATE_BIG (9) #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#define BLE_LL_STATE_CS (10) +#endif /* LL Features */ #define BLE_LL_FEAT_LE_ENCRYPTION (0x0000000000001) diff --git a/nimble/controller/include/controller/ble_ll_conn.h b/nimble/controller/include/controller/ble_ll_conn.h index 2b0c606dd6..c99559d6e2 100644 --- a/nimble/controller/include/controller/ble_ll_conn.h +++ b/nimble/controller/include/controller/ble_ll_conn.h @@ -202,6 +202,10 @@ struct ble_ll_conn_subrate_req_params { uint16_t supervision_tmo; }; +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +struct ble_ll_cs_sm; +#endif + /* Connection state machine */ struct ble_ll_conn_sm { @@ -274,7 +278,7 @@ struct ble_ll_conn_sm uint8_t vers_nr; uint8_t conn_features; uint8_t remote_features[7]; - uint16_t pending_ctrl_procs; + uint32_t pending_ctrl_procs; uint16_t event_cntr; uint16_t completed_pkts; uint16_t comp_id; @@ -400,6 +404,10 @@ struct ble_ll_conn_sm uint16_t css_slot_idx_pending; uint8_t css_period_idx; #endif + +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + struct ble_ll_cs_sm *cssm; +#endif }; /* Role */ diff --git a/nimble/controller/include/controller/ble_ll_cs.h b/nimble/controller/include/controller/ble_ll_cs.h index fb7cb4a59b..3f2c6bf321 100644 --- a/nimble/controller/include/controller/ble_ll_cs.h +++ b/nimble/controller/include/controller/ble_ll_cs.h @@ -28,6 +28,35 @@ extern "C" { #endif +void ble_ll_cs_init(void); +void ble_ll_cs_reset(void); + +void ble_ll_cs_capabilities_pdu_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_config_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_security_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_start_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_terminate_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr); + +int ble_ll_cs_rx_capabilities_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +void ble_ll_cs_rx_capabilities_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_rx_capabilities_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error); +int ble_ll_cs_rx_fae_req(struct ble_ll_conn_sm *connsm, struct os_mbuf *om); +void ble_ll_cs_rx_fae_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_rx_fae_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error); +int ble_ll_cs_rx_config_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +void ble_ll_cs_rx_config_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_rx_config_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error); +int ble_ll_cs_rx_security_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +void ble_ll_cs_rx_security_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_rx_security_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error); +int ble_ll_cs_rx_cs_start_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +int ble_ll_cs_rx_cs_start_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +int ble_ll_cs_rx_cs_start_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +void ble_ll_cs_rx_cs_start_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error); +int ble_ll_cs_rx_cs_terminate_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf); +void ble_ll_cs_rx_cs_terminate_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr); +void ble_ll_cs_rx_cs_terminate_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error); + /* HCI handlers */ int ble_ll_cs_hci_rd_loc_supp_cap(uint8_t *rspbuf, uint8_t *rsplen); int ble_ll_cs_hci_rd_rem_supp_cap(const uint8_t *cmdbuf, uint8_t cmdlen); @@ -44,6 +73,15 @@ int ble_ll_cs_hci_proc_enable(const uint8_t *cmdbuf, uint8_t cmdlen); int ble_ll_cs_hci_test(const uint8_t *cmdbuf, uint8_t cmdlen, uint8_t *rspbuf, uint8_t *rsplen); int ble_ll_cs_hci_test_end(void); +uint8_t ble_ll_cs_rtt_tx_make(uint8_t *dptr, uint8_t *hdr_byte); +void ble_ll_cs_sync_wfr_timer_exp(void); +void ble_ll_cs_sync_tx_end(struct ble_phy_cs_sync_results *results); +void ble_ll_cs_sync_rx_end(struct ble_phy_cs_sync_results *results); +void ble_ll_cs_tone_tx_end(struct ble_phy_cs_tone_results *results); +void ble_ll_cs_tone_rx_end(struct ble_phy_cs_tone_results *results); +void ble_ll_cs_proc_rm_from_sched(void *cb_arg); +void ble_ll_cs_proc_halt(void); + #ifdef __cplusplus } #endif diff --git a/nimble/controller/include/controller/ble_ll_ctrl.h b/nimble/controller/include/controller/ble_ll_ctrl.h index 89502a45a3..63585056eb 100644 --- a/nimble/controller/include/controller/ble_ll_ctrl.h +++ b/nimble/controller/include/controller/ble_ll_ctrl.h @@ -43,7 +43,13 @@ extern "C" { #define BLE_LL_CTRL_PROC_CIS_CREATE (11) #define BLE_LL_CTRL_PROC_SUBRATE_REQ (12) #define BLE_LL_CTRL_PROC_SUBRATE_UPDATE (13) -#define BLE_LL_CTRL_PROC_NUM (14) +#define BLE_LL_CTRL_PROC_CS_CAP_XCHG (14) +#define BLE_LL_CTRL_PROC_CS_FAE_REQ (15) +#define BLE_LL_CTRL_PROC_CS_CONF (16) +#define BLE_LL_CTRL_PROC_CS_SEC_START (17) +#define BLE_LL_CTRL_PROC_CS_START (18) +#define BLE_LL_CTRL_PROC_CS_TERMINATE (19) +#define BLE_LL_CTRL_PROC_NUM (20) #define BLE_LL_CTRL_PROC_IDLE (255) /* Checks if a particular control procedure is running */ @@ -100,14 +106,33 @@ extern "C" { #define BLE_LL_CTRL_SUBRATE_IND (0x27) #define BLE_LL_CTRL_CHAN_REPORTING_IND (0x28) #define BLE_LL_CTRL_CHAN_STATUS_IND (0x29) +#define BLE_LL_CTRL_PERIODIC_SYNC_WR_IND (0x2A) +#define BLE_LL_CTRL_FEATURE_EXT_REQ (0x2B) +#define BLE_LL_CTRL_FEATURE_EXT_RSP (0x2C) +#define BLE_LL_CTRL_CS_SEC_RSP (0x2D) +#define BLE_LL_CTRL_CS_CAPABILITIES_REQ (0x2E) +#define BLE_LL_CTRL_CS_CAPABILITIES_RSP (0x2F) +#define BLE_LL_CTRL_CS_CONFIG_REQ (0x30) +#define BLE_LL_CTRL_CS_CONFIG_RSP (0x31) +#define BLE_LL_CTRL_CS_REQ (0x32) +#define BLE_LL_CTRL_CS_RSP (0x33) +#define BLE_LL_CTRL_CS_IND (0x34) +#define BLE_LL_CTRL_CS_TERMINATE_REQ (0x35) +#define BLE_LL_CTRL_CS_FAE_REQ (0x36) +#define BLE_LL_CTRL_CS_FAE_RSP (0x37) +#define BLE_LL_CTRL_CS_CHANNEL_MAP_IND (0x38) +#define BLE_LL_CTRL_CS_SEC_REQ (0x39) +#define BLE_LL_CTRL_CS_TERMINATE_RSP (0x3A) /* Maximum opcode value */ -#define BLE_LL_CTRL_OPCODES (BLE_LL_CTRL_CHAN_STATUS_IND + 1) +#define BLE_LL_CTRL_OPCODES (BLE_LL_CTRL_CS_TERMINATE_RSP + 1) extern const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES]; /* Maximum LL control PDU size */ -#if MYNEWT_VAL(BLE_ISO) +#if MYNEWT_VAL(BLE_CHANNEL_SOUNDING) +#define BLE_LL_CTRL_MAX_PDU_LEN (73) +#elif MYNEWT_VAL(BLE_ISO) #define BLE_LL_CTRL_MAX_PDU_LEN (42) #elif MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) #define BLE_LL_CTRL_MAX_PDU_LEN (35) @@ -292,6 +317,27 @@ struct ble_ll_len_req #define BLE_LL_CTRL_CHAN_REPORTING_IND_LEN (3) #define BLE_LL_CTRL_CHAN_STATUS_IND_LEN (10) +/* Not implemented */ +#define BLE_LL_CTRL_PERIODIC_SYNC_WR_IND_LEN (0) +#define BLE_LL_CTRL_FEATURE_EXT_REQ_LEN (0) +#define BLE_LL_CTRL_FEATURE_EXT_RSP_LEN (0) + +/* Channel Sounding */ +#define BLE_LL_CTRL_CS_SEC_RSP_LEN (20) +#define BLE_LL_CTRL_CS_CAPABILITIES_REQ_LEN (25) +#define BLE_LL_CTRL_CS_CAPABILITIES_RSP_LEN (25) +#define BLE_LL_CTRL_CS_CONFIG_REQ_LEN (27) +#define BLE_LL_CTRL_CS_CONFIG_RSP_LEN (1) +#define BLE_LL_CTRL_CS_REQ_LEN (28) +#define BLE_LL_CTRL_CS_RSP_LEN (21) +#define BLE_LL_CTRL_CS_IND_LEN (18) +#define BLE_LL_CTRL_CS_TERMINATE_REQ_LEN (4) +#define BLE_LL_CTRL_CS_FAE_REQ_LEN (0) +#define BLE_LL_CTRL_CS_FAE_RSP_LEN (72) +#define BLE_LL_CTRL_CS_CHANNEL_MAP_IND_LEN (12) +#define BLE_LL_CTRL_CS_SEC_REQ_LEN (20) +#define BLE_LL_CTRL_CS_TERMINATE_RSP_LEN (4) + /* API */ struct ble_ll_conn_sm; void ble_ll_ctrl_proc_start(struct ble_ll_conn_sm *connsm, int ctrl_proc, diff --git a/nimble/controller/include/controller/ble_ll_sched.h b/nimble/controller/include/controller/ble_ll_sched.h index 5daa647fab..0d3d75ed51 100644 --- a/nimble/controller/include/controller/ble_ll_sched.h +++ b/nimble/controller/include/controller/ble_ll_sched.h @@ -71,6 +71,7 @@ extern uint8_t g_ble_ll_sched_offset_ticks; #define BLE_LL_SCHED_TYPE_SYNC (7) #define BLE_LL_SCHED_TYPE_SCAN_AUX (8) #define BLE_LL_SCHED_TYPE_BIG (9) +#define BLE_LL_SCHED_TYPE_CS (10) #if MYNEWT_VAL(BLE_LL_EXT) #define BLE_LL_SCHED_TYPE_EXTERNAL (255) #endif @@ -222,6 +223,10 @@ uint32_t ble_ll_sched_css_get_conn_interval_us(void); int ble_ll_sched_iso_big(struct ble_ll_sched_item *sch, int first, int fixed); #endif /* BLE_LL_ISO_BROADCASTER */ +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +int ble_ll_sched_cs_proc(struct ble_ll_sched_item *sch); +#endif /* BLE_LL_CHANNEL_SOUNDING */ + #ifdef __cplusplus } #endif diff --git a/nimble/controller/pkg.yml b/nimble/controller/pkg.yml index 2f22e3ebf5..e5ba7b600c 100644 --- a/nimble/controller/pkg.yml +++ b/nimble/controller/pkg.yml @@ -42,7 +42,14 @@ pkg.deps: - nimble - nimble/transport +pkg.deps.TEST: + - "@apache-mynewt-core/crypto/mbedtls" + pkg.init: ble_ll_init: - $before:ble_transport_hs_init - $before:ble_transport_ll_init + +pkg.cflags.TEST: + - -Irepos/mbedtls/include + - -Irepos/include/mbedtls diff --git a/nimble/controller/src/ble_ll.c b/nimble/controller/src/ble_ll.c index a0d6dbbc03..b35b819b2c 100644 --- a/nimble/controller/src/ble_ll.c +++ b/nimble/controller/src/ble_ll.c @@ -52,6 +52,9 @@ #if MYNEWT_VAL(BLE_LL_ISO_BROADCASTER) #include "controller/ble_ll_iso_big.h" #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include "controller/ble_ll_cs.h" +#endif #if MYNEWT_VAL(BLE_LL_EXT) #include "controller/ble_ll_ext.h" #endif @@ -1704,6 +1707,10 @@ ble_ll_reset(void) ble_ll_iso_reset(); #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + ble_ll_cs_reset(); +#endif + /* Re-initialize the PHY */ rc = ble_phy_init(); @@ -1990,6 +1997,10 @@ ble_ll_init(void) ble_ll_iso_big_init(); #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + ble_ll_cs_init(); +#endif + #if MYNEWT_VAL(BLE_LL_EXT) ble_ll_ext_init(); #endif diff --git a/nimble/controller/src/ble_ll_conn.c b/nimble/controller/src/ble_ll_conn.c index 52446b6471..b3d3b754ce 100644 --- a/nimble/controller/src/ble_ll_conn.c +++ b/nimble/controller/src/ble_ll_conn.c @@ -1964,6 +1964,11 @@ ble_ll_conn_set_csa(struct ble_ll_conn_sm *connsm, bool chsel) connsm->data_chan_index = ble_ll_conn_calc_dci(connsm, 1); } +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +void ble_ll_cs_sm_init(struct ble_ll_conn_sm *connsm); +void ble_ll_cs_sm_free(struct ble_ll_conn_sm *connsm); +#endif + /** * Create a new connection state machine. This is done once per * connection when the HCI command "create connection" is issued to the @@ -2086,6 +2091,10 @@ ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm) ble_ll_conn_css_update_list(connsm); } #endif + +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + ble_ll_cs_sm_init(connsm); +#endif } void @@ -2211,6 +2220,10 @@ ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err) } #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + ble_ll_cs_sm_free(connsm); +#endif + #if MYNEWT_VAL(BLE_LL_CFG_FEAT_CTRL_TO_HOST_FLOW_CONTROL) ble_ll_conn_cth_flow_free_credit(connsm, connsm->cth_flow_pending); #endif @@ -2289,7 +2302,7 @@ ble_ll_conn_anchor_get(struct ble_ll_conn_sm *connsm, uint16_t *event_cntr, *anchor_usecs = connsm->anchor_point_usecs; } -#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) +#if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PERIODIC_ADV_SYNC_TRANSFER) || MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) void ble_ll_conn_anchor_event_cntr_get(struct ble_ll_conn_sm *connsm, uint16_t event_cntr, uint32_t *anchor, @@ -3971,9 +3984,13 @@ ble_ll_conn_enqueue_pkt(struct ble_ll_conn_sm *connsm, struct os_mbuf *om, return; } - /* Set mbuf length and packet length if a control PDU */ + /* Set overall packet length if a control PDU */ if (hdr_byte == BLE_LL_LLID_CTRL) { - om->om_len = length; + /* Set mbuf length if not chained mbufs */ + if (SLIST_NEXT(om, om_next) == NULL) { + om->om_len = length; + } + OS_MBUF_PKTHDR(om)->omp_len = length; num_pkt = 0; } else { diff --git a/nimble/controller/src/ble_ll_cs.c b/nimble/controller/src/ble_ll_cs.c index a9a181a849..fc5e2c442c 100644 --- a/nimble/controller/src/ble_ll_cs.c +++ b/nimble/controller/src/ble_ll_cs.c @@ -26,81 +26,1814 @@ #include "controller/ble_ll_conn.h" #include "controller/ble_ll_hci.h" #include "controller/ble_ll_cs.h" +#include "controller/ble_ll_tmr.h" +#include "ble_ll_conn_priv.h" +#include "ble_ll_cs_priv.h" +#include "os/os_mbuf.h" + +#define T_IP1_CAP_ID_10US 0 +#define T_IP1_CAP_ID_20US 1 +#define T_IP1_CAP_ID_30US 2 +#define T_IP1_CAP_ID_40US 3 +#define T_IP1_CAP_ID_50US 4 +#define T_IP1_CAP_ID_60US 5 +#define T_IP1_CAP_ID_80US 6 +#define T_IP1_CAP_ID_145US 7 + +#define T_IP2_CAP_ID_10US 0 +#define T_IP2_CAP_ID_20US 1 +#define T_IP2_CAP_ID_30US 2 +#define T_IP2_CAP_ID_40US 3 +#define T_IP2_CAP_ID_50US 4 +#define T_IP2_CAP_ID_60US 5 +#define T_IP2_CAP_ID_80US 6 +#define T_IP2_CAP_ID_145US 7 + +#define T_FCS_CAP_ID_15US 0 +#define T_FCS_CAP_ID_20US 1 +#define T_FCS_CAP_ID_30US 2 +#define T_FCS_CAP_ID_40US 3 +#define T_FCS_CAP_ID_50US 4 +#define T_FCS_CAP_ID_60US 5 +#define T_FCS_CAP_ID_80US 6 +#define T_FCS_CAP_ID_100US 7 +#define T_FCS_CAP_ID_120US 8 +#define T_FCS_CAP_ID_150US 9 + +#define T_PM_CAP_ID_10US 0 +#define T_PM_CAP_ID_20US 1 +#define T_PM_CAP_ID_40US 2 + +struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap; +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]; +uint8_t g_ble_ll_cs_chan_count = 0; +uint8_t g_ble_ll_cs_chan_indices[72]; + +#define MIN_CONN_EVENT_COUNT_BEFORE_START 10 +#define OFFSET_FROM_CONN_EVENT_TICKS 10 +#define OFFSET_MIN_USECS 500 +#define OFFSET_MAX_USECS 4000000 + +extern int8_t g_ble_ll_tx_power; +void ble_ll_ctrl_rej_ext_ind_make(uint8_t rej_opcode, uint8_t err, uint8_t *ctrdata); 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 & ~(1 << T_IP1_CAP_ID_145US)); + rsp->optional_t_ip2_times_supported = htole16(cap->t_ip2_capability & ~(1 << T_IP2_CAP_ID_145US)); + rsp->optional_t_fcs_times_supported = htole16(cap->t_fcs_capability & ~(1 << T_FCS_CAP_ID_150US)); + rsp->optional_t_pm_times_supported = htole16(cap->t_pm_capability & ~(1 << T_PM_CAP_ID_40US)); + 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 & ~(1 << T_IP1_CAP_ID_145US)); + put_le16(dptr + 18, cap->t_ip2_capability & ~(1 << T_IP2_CAP_ID_145US)); + put_le16(dptr + 20, cap->t_fcs_capability & ~(1 << T_FCS_CAP_ID_150US)); + put_le16(dptr + 22, cap->t_pm_capability & ~(1 << T_PM_CAP_ID_40US)); + 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 & 0x01; + cap->rtt_capability = dptr[1] & 0x05; + 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) & 0x01; + cap->nadm_random_sequence_capability = get_le16(dptr + 7) & 0x01; + cap->cs_sync_phy_capability = dptr[9] & 0x06; + + 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) & 0x00FF) | (1 << T_IP1_CAP_ID_145US); + cap->t_ip2_capability = (get_le16(dptr + 18) & 0x00FF) | (1 << T_IP1_CAP_ID_145US); + cap->t_fcs_capability = (get_le16(dptr + 20) & 0x03FF) | (1 << T_FCS_CAP_ID_150US); + cap->t_pm_capability = (get_le16(dptr + 22) & 0x07) | (1 << T_PM_CAP_ID_40US); + cap->tx_snr_capablity = (dptr[24] >> 1) & 0b01111111; + + /* The capabilites contain info about allowed values for + * CS procedures. Ignore the RFU values here. + * We will be able to reject/renegotiate unsupported values + * if the remote controller will use them in the procedures. + */ + + if (cap->number_of_antennas > 4) { + cap->number_of_antennas = 4; + } + + if (cap->max_number_of_antenna_paths > 4) { + cap->max_number_of_antenna_paths = 4; + } + + if (cap->max_number_of_antenna_paths < cap->number_of_antennas) { + cap->number_of_antennas = cap->max_number_of_antenna_paths; + } + + if (cap->max_number_of_configs > 4) { + cap->max_number_of_configs = 4; + } + + if (!(cap->t_sw == 0x00 || + cap->t_sw == 0x01 || + cap->t_sw == 0x02 || + cap->t_sw == 0x04 || + cap->t_sw == 0x0A)) { + /* If the remote does not support a valid duration of the antenna switch period, + * lets assume it does not support the antenna switching at all. + */ + cap->number_of_antennas = 1; + cap->t_sw = 0; + } +} + +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; + + memset(ev, 0, sizeof(*ev)); + ev->subev_code = BLE_HCI_LE_SUBEV_CS_RD_REM_SUPP_CAP_COMPLETE; + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + + if (status == BLE_ERR_SUCCESS) { + 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 & ~(1 << T_IP1_CAP_ID_145US)); + ev->optional_t_ip2_times_supported = htole16(cap->t_ip2_capability & ~(1 << T_IP2_CAP_ID_145US)); + ev->optional_t_fcs_times_supported = htole16(cap->t_fcs_capability & ~(1 << T_FCS_CAP_ID_150US)); + ev->optional_t_pm_times_supported = htole16(cap->t_pm_capability & ~(1 << T_PM_CAP_ID_40US)); + 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_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error) +{ + /* 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_error); +} + +void +ble_ll_cs_rx_capabilities_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CAP_XCHG)) { + /* Ignore */ + return; + } + + ble_ll_cs_update_rem_capabilities(connsm, dptr); + + /* 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) | (1 << T_IP1_CAP_ID_145US); + cap->t_ip2_capability = le16toh(cmd->optional_t_ip2_times_supported) | (1 << T_IP2_CAP_ID_145US); + cap->t_fcs_capability = le16toh(cmd->optional_t_fcs_times_supported) | (1 << T_FCS_CAP_ID_150US); + cap->t_pm_capability = le16toh(cmd->optional_t_pm_times_supported) | (1 << T_PM_CAP_ID_40US); + 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; +} + +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) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_SEC_REQ, + BLE_ERR_INSUFFICIENT_SEC, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + /* 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); + + connsm->cssm->start_procedure_count = 0; + + 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)) { + /* Ignore */ + 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); + + connsm->cssm->start_procedure_count = 0; + + /* 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_rx_security_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error) +{ + /* 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, ble_error); +} + +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; + uint8_t i; + + 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 (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; + } + + /* 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; + } + + cssm->roles_enabled = cmd->role_enable; + cssm->cs_sync_antenna_selection = cmd->cs_sync_antenna_selection; + + 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, struct os_mbuf *om) +{ + /* Space for response code */ + om->om_len = 1; + OS_MBUF_PKTLEN(om) = om->om_len; + os_mbuf_append(om, 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; + + memset(ev, 0, sizeof(*ev)); + ev->subev_code = BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_COMPLETE; + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + + if (status == BLE_ERR_SUCCESS) { + 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)) { + /* Ignore */ + 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); +} + +void +ble_ll_cs_rx_fae_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error) +{ + /* 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_error); } 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; + + assert(config_id < ARRAY_SIZE(connsm->cssm->config)); + + *dptr = config_id | action << 6; + + if (action == 0x00) { + /* Removing the config, all remaining fields are RFU. */ + memset(dptr + 1, 0, 26); + + return; + } + + conf = &connsm->cssm->tmp_config; + 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_index; + dptr[23] = conf->t_ip2_index; + dptr[24] = conf->t_fcs_index; + dptr[25] = conf->t_pm_index; + /* RFU octet */ + dptr[26] = 0x00; +} + +static int +ble_ll_cs_verify_config(struct ble_ll_cs_config *conf) +{ + if (conf->chan_map[9] & 0x80) { + return 1; + } + + if (conf->chan_map_repetition < 1) { + return 1; + } + + /* Valid combinations of Main_Mode and Sub_Mode selections */ + if (conf->main_mode == 0x01) { + if (conf->sub_mode != 0xFF) { + return 1; + } + } else if (conf->main_mode == 0x02) { + if (conf->sub_mode != 0x01 && + conf->sub_mode != 0x03 && + conf->sub_mode != 0xFF) { + return 1; + } + } else if (conf->main_mode == 0x03) { + if (conf->sub_mode != 0x02 && + conf->sub_mode != 0xFF) { + return 1; + } + } else { + return 1; + } + + if (conf->sub_mode == 0xFF) { + /* RFU if Sub_Mode is None */ + conf->main_mode_min_steps = 0x00; + conf->main_mode_max_steps = 0x00; + } + + if (conf->main_mode_repetition > 0x03) { + return 1; + } + + if (conf->mode_0_steps < 1 || conf->mode_0_steps > 3) { + return 1; + } + + if (conf->cs_sync_phy & 0xF0) { + return 1; + } + + if (conf->rtt_type > 0x06) { + return 1; + } + + if (conf->chan_sel > 0x01) { + return 1; + } + + if (conf->chan_sel == 0x01) { + if (conf->ch3cshape > 0x01) { + return 1; + } + + if (!IN_RANGE(conf->ch3cjump, 2, 8)) { + return 1; + } + } + + if (conf->t_ip1_index > 7) { + return 1; + } + + if (conf->t_ip2_index > 7) { + return 1; + } + + if (conf->t_fcs_index > 9) { + return 1; + } + + if (conf->t_pm_index > 2) { + return 1; + } + + return 0; +} + +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 && status == BLE_ERR_SUCCESS) { + 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); + } + } +} + +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; + struct ble_ll_cs_sm *cssm = connsm->cssm; + + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF)) { + if (CONN_IS_CENTRAL(connsm)) { + /* Reject CS config initiated by peripheral */ + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ, + BLE_ERR_LMP_COLLISION, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } else { + /* Take no further action in the Peripheral-initiated procedure + * and proceed to handle the Central-initiated procedure. + */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_CONF); + } + } + + if (config_id >= ARRAY_SIZE(cssm->config)) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + conf = &cssm->config[config_id]; + if (conf->config_in_use) { + /* CS procedure in progress exit with error */ + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ, + BLE_ERR_CMD_DISALLOWED, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + /* Respond with LL_CS_CONFIG_RSP PDU */ + *rspbuf = config_id | action << 6; + + if (action == 0x00) { + /* CS configuration removed. */ + memset(conf, 0, sizeof(*conf)); + + return BLE_LL_CTRL_CS_CONFIG_RSP; + } + + conf = &cssm->tmp_config; + memset(conf, 0, sizeof(*conf)); + 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_index = dptr[22]; + conf->t_ip2_index = dptr[23]; + conf->t_fcs_index = dptr[24]; + conf->t_pm_index = dptr[25]; + + if (ble_ll_cs_verify_config(conf)) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_CONFIG_REQ, + BLE_ERR_UNSUPP_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + conf->t_ip1 = t_ip1[conf->t_ip1_index]; + conf->t_ip2 = t_ip2[conf->t_ip2_index]; + conf->t_fcs = t_fcs[conf->t_fcs_index]; + conf->t_pm = t_pm[conf->t_pm_index]; + conf->config_enabled = 1; + + memcpy(&cssm->config[config_id], conf, sizeof(*conf)); + memset(conf, 0, sizeof(*conf)); + + return BLE_LL_CTRL_CS_CONFIG_RSP; +} + +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; + struct ble_ll_cs_sm *cssm = connsm->cssm; + + if (config_id != cssm->config_req_id || + !IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF)) { + return; + } + + /* Configure CS config locally */ + memcpy(&cssm->config[config_id], &cssm->tmp_config, sizeof(cssm->tmp_config)); + memset(&cssm->tmp_config, 0, sizeof(cssm->tmp_config)); + + /* 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); +} + +void +ble_ll_cs_rx_config_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error) +{ + struct ble_ll_cs_sm *cssm = connsm->cssm; + + memset(&cssm->tmp_config, 0, sizeof(cssm->tmp_config)); + + /* 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, cssm->config_req_id, + cssm->config_req_action, ble_error); +} + +static int +ble_ll_cs_select_capability(uint8_t capability_values_count, + uint8_t *out_index, uint16_t local_capability, + uint16_t remote_capability) +{ + uint16_t common_capability = local_capability & remote_capability; + uint8_t i; + + for (i = 0; i < capability_values_count; i++) { + if ((common_capability >> i) & 1) { + *out_index = 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_sm *cssm; + struct ble_ll_cs_config *conf; + + 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; + } + + cssm = connsm->cssm; + if (cmd->config_id >= ARRAY_SIZE(cssm->config)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + conf = &cssm->config[cmd->config_id]; + + /* If already pending or CS procedure in progress exit with error */ + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF) || + conf->config_in_use) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Save the CS configuration in temporary variable as the config + * might be rejected by the remote. + */ + conf = &cssm->tmp_config; + memset(conf, 0, sizeof(*conf)); + conf->config_enabled = 1; + 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(ARRAY_SIZE(t_ip1), &conf->t_ip1_index, + 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(ARRAY_SIZE(t_ip2), &conf->t_ip2_index, + 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(ARRAY_SIZE(t_fcs), &conf->t_fcs_index, + 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(ARRAY_SIZE(t_pm), &conf->t_pm_index, + 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; + } + + conf->t_ip1 = t_ip1[conf->t_ip1_index]; + conf->t_ip2 = t_ip2[conf->t_ip2_index]; + conf->t_fcs = t_fcs[conf->t_fcs_index]; + conf->t_pm = t_pm[conf->t_pm_index]; + + if (ble_ll_cs_verify_config(conf)) { + assert(0); + 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)) { + 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 (cmd->config_id >= ARRAY_SIZE(cssm->config)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + conf = &cssm->config[cmd->config_id]; + + /* If already pending or CS procedure in progress exit with error */ + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_CONF) || + conf->config_in_use) { + return BLE_ERR_CMD_DISALLOWED; + } + + /* Remove the CS config locally */ + 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)) { + 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 (cmd->config_id >= ARRAY_SIZE(connsm->cssm->config)) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + conf = &connsm->cssm->config[cmd->config_id]; + + /* If CS configuration with Config_ID does not exists */ + if (!conf->config_enabled) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + /* If CS measurement is enabled exit with error */ + if (connsm->cssm->measurement_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (g_ble_ll_cs_chan_count < 15) { + return BLE_ERR_INSUFFICIENT_CHAN; + } + + params = &conf->pref_proc_params; + params->max_procedure_len = htole16(cmd->max_procedure_len); + params->min_procedure_interval = htole16(cmd->min_procedure_interval); + params->max_procedure_interval = htole16(cmd->max_procedure_interval); + params->max_procedure_count = htole16(cmd->max_procedure_count); + params->min_subevent_len = get_le24(cmd->min_subevent_len); + params->max_subevent_len = get_le24(cmd->max_subevent_len); + params->aci = cmd->tone_antenna_config_selection; + params->phy = cmd->phy; + params->tx_power_delta = cmd->tx_power_delta; + params->preferred_peer_antenna = cmd->preferred_peer_antenna; + params->snr_control_initiator = cmd->snr_control_initiator & 0x0F; + params->snr_control_reflector = cmd->snr_control_reflector & 0x0F; + params->params_ready = 1; + + rsp->conn_handle = cmd->conn_handle; + *rsplen = sizeof(*rsp); + + return BLE_ERR_SUCCESS; } int ble_ll_cs_hci_proc_enable(const uint8_t *cmdbuf, uint8_t cmdlen) { - return BLE_ERR_UNSUPPORTED; + const struct ble_hci_le_cs_proc_enable_cp *cmd = (const void *)cmdbuf; + struct ble_ll_conn_sm *connsm; + struct ble_ll_cs_config *conf; + + if (cmdlen != sizeof(*cmd) || cmd->config_id >= ARRAY_SIZE(connsm->cssm->config)) { + 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]; + if (!conf->config_enabled) { + return BLE_ERR_INV_HCI_CMD_PARMS; + } + + if (cmd->enable) { + if (!conf->pref_proc_params.params_ready || + connsm->cssm->measurement_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + if (g_ble_ll_cs_chan_count < 15) { + return BLE_ERR_INSUFFICIENT_CHAN; + } + + /* Start scheduling CS procedures */ + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_START, NULL); + } else { + if (!connsm->cssm->measurement_enabled) { + return BLE_ERR_CMD_DISALLOWED; + } + + connsm->cssm->terminate_measurement = 1; + connsm->cssm->terminate_config_id = cmd->config_id; + connsm->cssm->terminate_error_code = BLE_ERR_SUCCESS; + + /* Terminate the CS measurement early */ + ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_CS_TERMINATE, NULL); + } + + return BLE_ERR_SUCCESS; +} + +static void +ble_ll_cs_ev_cs_proc_enable_complete(struct ble_ll_conn_sm *connsm, + uint8_t config_id, uint8_t status) +{ + struct ble_hci_ev_le_subev_cs_proc_enable_complete *ev; + const struct ble_ll_cs_proc_params *params; + struct ble_hci_ev *hci_ev; + + if (ble_ll_hci_is_le_event_enabled( + BLE_HCI_LE_SUBEV_CS_PROC_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; + + memset(ev, 0, sizeof(*ev)); + ev->subev_code = BLE_HCI_LE_SUBEV_CS_PROC_ENABLE_COMPLETE; + ev->status = status; + ev->conn_handle = htole16(connsm->conn_handle); + ev->config_id = config_id; + ev->state = connsm->cssm->measurement_enabled; + + if (status == BLE_ERR_SUCCESS) { + params = &connsm->cssm->config[config_id].proc_params; + ev->tone_antenna_config_selection = params->aci; + ev->selected_tx_power = g_ble_ll_tx_power; + put_le24(ev->subevent_len, params->subevent_len); + ev->subevents_per_event = params->subevents_per_event; + ev->subevent_interval = htole16(params->subevent_interval); + ev->event_interval = htole16(params->event_interval); + ev->procedure_interval = htole16(params->procedure_interval); + ev->procedure_count = htole16(params->max_procedure_count); + } + + ble_ll_hci_event_send(hci_ev); + } + } +} + +void +ble_ll_cs_start_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + uint8_t config_id = connsm->cssm->config_req_id; + uint8_t subevents_per_event; + uint16_t anchor_conn_event_cntr = connsm->event_cntr + MIN_CONN_EVENT_COUNT_BEFORE_START; + struct ble_ll_cs_pref_proc_params *params; + struct ble_ll_cs_proc_params *ps; + uint32_t offset_ticks; + uint32_t offset; + uint32_t offset_min = OFFSET_MIN_USECS; + uint32_t offset_max = OFFSET_MAX_USECS; + uint32_t subevent_len; + /* Handle only one CS procedure for now */ + uint16_t procedure_interval = 0; + uint16_t event_interval; + uint16_t subevent_interval; + + assert(config_id < ARRAY_SIZE(connsm->cssm->config)); + params = &connsm->cssm->config[config_id].pref_proc_params; + subevent_len = params->min_subevent_len; + + /* Select the offset so that it will not overlap with a connection event */ + offset_ticks = connsm->conn_sch.end_time - connsm->conn_sch.start_time; + offset = ble_ll_tmr_t2u(offset_ticks + OFFSET_FROM_CONN_EVENT_TICKS); + + if (offset_min < offset) { + offset_min = offset; + } + offset_max = ble_ll_tmr_t2u(connsm->conn_itvl_ticks); + + /* TODO: There can be multiple scheduling approaches. + * For now use a simple one: schedule one CS subevent per CS event, + * the subevent takes whole CS event, the subevent do not exceed + * the underlying LE connection interval. + * The number of subevents translates into the frequency of sending + * subevent result events to the host. Each new CS event/subevent can help + * get the sync back on track in case of de-sync. + */ + event_interval = 1; + subevents_per_event = 1; + subevent_interval = 0; + + /* LL_CS_REQ parameters are just a suggestion and may be resuggested + * by the peer in LL_CS_RSP. + */ + + ps = &connsm->cssm->config[config_id].proc_params; + memset(ps, 0, sizeof(*ps)); + ps->max_procedure_len = params->max_procedure_len; + ps->procedure_interval = procedure_interval; + ps->max_procedure_count = params->max_procedure_count; + ps->preferred_peer_antenna = params->preferred_peer_antenna; + ps->tx_snr_i = params->snr_control_initiator; + ps->tx_snr_r = params->snr_control_reflector; + + *dptr = config_id; + put_le16(dptr + 1, anchor_conn_event_cntr); + put_le24(dptr + 3, offset_min); + put_le24(dptr + 6, offset_max); + put_le16(dptr + 9, params->max_procedure_len); + put_le16(dptr + 11, event_interval); + dptr[13] = subevents_per_event; + put_le16(dptr + 14, subevent_interval); + put_le24(dptr + 16, subevent_len); + put_le16(dptr + 19, procedure_interval); + put_le16(dptr + 21, params->max_procedure_count); + dptr[23] = params->aci; + dptr[24] = params->preferred_peer_antenna; + dptr[25] = params->phy; + dptr[26] = params->tx_power_delta; + dptr[27] = params->snr_control_initiator | + params->snr_control_reflector << 4; +} + +static int +ble_ll_cs_start_ind_make(struct ble_ll_conn_sm *connsm, uint8_t *rspbuf) +{ + uint8_t config_id = connsm->cssm->config_req_id; + struct ble_ll_cs_proc_params *ps; + + assert(config_id < ARRAY_SIZE(connsm->cssm->config)); + ps = &connsm->cssm->config[config_id].proc_params; + + *rspbuf = config_id; + put_le16(rspbuf + 1, ps->anchor_conn_event_cntr); + put_le24(rspbuf + 3, ps->event_offset); + put_le16(rspbuf + 6, ps->event_interval); + rspbuf[8] = ps->subevents_per_event; + put_le16(rspbuf + 9, ps->subevent_interval); + put_le24(rspbuf + 11, ps->subevent_len); + rspbuf[14] = ps->aci; + rspbuf[15] = ps->phy; + rspbuf[16] = ps->tx_power_delta; + rspbuf[17] = 0x00; + + return BLE_LL_CTRL_CS_IND; +} + +static int +ble_ll_cs_start_rsp_make(struct ble_ll_conn_sm *connsm, uint8_t *rspbuf) +{ + uint8_t config_id = connsm->cssm->config_req_id; + struct ble_ll_cs_proc_params *ps; + + assert(config_id < ARRAY_SIZE(connsm->cssm->config)); + ps = &connsm->cssm->config[config_id].proc_params; + + *rspbuf = config_id; + put_le16(rspbuf + 1, ps->anchor_conn_event_cntr); + put_le24(rspbuf + 3, ps->offset_min); + put_le24(rspbuf + 6, ps->offset_max); + put_le16(rspbuf + 9, ps->event_interval); + rspbuf[11] = ps->subevents_per_event; + put_le16(rspbuf + 12, ps->subevent_interval); + put_le24(rspbuf + 14, ps->subevent_len); + rspbuf[17] = ps->aci; + rspbuf[18] = ps->phy; + rspbuf[19] = ps->tx_power_delta; + /* 1 RFU octet */ + rspbuf[20] = 0x00; + + return BLE_LL_CTRL_CS_RSP; +} + +static int +validate_cs_start_parameters(struct ble_ll_conn_sm *connsm, + struct ble_ll_cs_proc_params *ps) +{ + int err = 0; + uint32_t conn_itvl_us = connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS; + + if (conn_itvl_us < ps->offset_max) { + /* TODO: Shall be less than the LE connection interval, but how much? */ + ps->offset_max = conn_itvl_us - 1; + err = 1; + } + + if (ps->offset_min < 500 || ps->offset_max < ps->offset_min) { + ps->offset_min = 500; + err = 1; + } + + if (ps->anchor_conn_event_cntr < connsm->event_cntr + 3) { + /* XXX: +3 to ensure that LL_CS_RSP and LL_CS_IND will make it on time + * before the first CS step. But maybe that's a bit of an exaggeration. + */ + ps->anchor_conn_event_cntr = connsm->event_cntr + 3; + err = 1; + } + + if (!IN_RANGE(ps->event_interval, BLE_LL_CS_EVENT_INTERVAL_MIN, + BLE_LL_CS_EVENT_INTERVAL_MAX)) { + ps->event_interval = BLE_LL_CS_EVENT_INTERVAL_MIN; + err = 1; + } + + if (!IN_RANGE(ps->subevents_per_event, BLE_LL_CS_SUBEVENTS_PER_PROCEDURE_MIN, + BLE_LL_CS_SUBEVENTS_PER_PROCEDURE_MAX)) { + ps->subevents_per_event = BLE_LL_CS_SUBEVENTS_PER_PROCEDURE_MIN; + err = 1; + } + + if (ps->subevents_per_event == 1) { + ps->subevent_interval = 0; + } else if (!IN_RANGE(ps->subevent_interval, BLE_LL_CS_SUBEVENTS_INTERVAL_MIN, + BLE_LL_CS_SUBEVENTS_INTERVAL_MAX)) { + ps->subevent_interval = BLE_LL_CS_SUBEVENTS_INTERVAL_MIN; + err = 1; + } + + if (!IN_RANGE(ps->subevent_len, BLE_LL_CS_SUBEVENT_LEN_MIN, + BLE_LL_CS_SUBEVENT_LEN_MAX)) { + ps->subevent_len = BLE_LL_CS_SUBEVENT_LEN_MIN; + err = 1; + } + + if (ps->aci > 7) { + /* Resuggest ACI with ID 0 */ + ps->aci = 0; + err = 1; + } + + if (ps->phy == 0) { + /* Resuggest using LE 1M PHY */ + ps->phy = 1; + err = 1; + } + + if (!(IN_RANGE(ps->tx_power_delta, 0x00, 0x14) || + IN_RANGE(ps->tx_power_delta, 0x7E, 0xFF))) { + /* Invalid range. Resuggest to not change the power level */ + ps->tx_power_delta = 0x00; + err = 1; + } + + if ((ps->tx_snr_i > 4 && ps->tx_snr_i != 0xF) || + (ps->tx_snr_r > 4 && ps->tx_snr_r != 0xF) || + ps->preferred_peer_antenna == 0) { + /* We cannot resuggest these, so reject them */ + return -1; + } + + return err; +} + +int +ble_ll_cs_rx_cs_start_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf) +{ + uint8_t config_id = *dptr & 0b00111111; + struct ble_ll_cs_config *conf; + struct ble_ll_cs_proc_params *ps; + int rc; + + /* TODO: take into account the struct ble_ll_cs_pref_proc_params *params */ + + if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_START)) { + if (CONN_IS_CENTRAL(connsm)) { + /* Reject CS config initiated by peripheral */ + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_REQ, + BLE_ERR_LMP_COLLISION, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } else { + /* Take no further action in the Peripheral-initiated procedure + * and proceed to handle the Central-initiated procedure. + */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_START); + } + } + + if (config_id >= ARRAY_SIZE(connsm->cssm->config)) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_REQ, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + conf = &connsm->cssm->config[config_id]; + + ps = &conf->proc_params; + memset(ps, 0, sizeof(*ps)); + + ps->anchor_conn_event_cntr = get_le16(dptr + 1); + ps->offset_min = get_le24(dptr + 3); + ps->offset_max = get_le24(dptr + 6); + ps->max_procedure_len = get_le16(dptr + 9); + ps->event_interval = get_le16(dptr + 11); + ps->subevents_per_event = dptr[13]; + ps->subevent_interval = get_le16(dptr + 14); + ps->subevent_len = get_le24(dptr + 16); + ps->procedure_interval = get_le16(dptr + 19); + ps->max_procedure_count = get_le16(dptr + 21); + ps->aci = dptr[23]; + ps->preferred_peer_antenna = dptr[24] & 0b00001111; + ps->phy = dptr[25] & 0b00001111; + ps->tx_power_delta = dptr[26]; + ps->tx_snr_i = dptr[27] & 0b00001111; + ps->tx_snr_r = (dptr[27] >> 4) & 0b00001111; + + /* In Peripheral role some parameters can be resuggested in LL_CS_RSP. */ + rc = validate_cs_start_parameters(connsm, ps); + if (rc < 0 || (rc > 0 && CONN_IS_CENTRAL(connsm))) { + memset(ps, 0, sizeof(*ps)); + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_REQ, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + /* In Central role reply with LL_CS_IND instead of LL_CS_RSP */ + if (CONN_IS_CENTRAL(connsm)) { + rc = ble_ll_cs_proc_scheduling_start(connsm, config_id); + if (rc) { + memset(ps, 0, sizeof(*ps)); + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_REQ, rc, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + ble_ll_cs_ev_cs_proc_enable_complete(connsm, config_id, BLE_ERR_SUCCESS); + + return ble_ll_cs_start_ind_make(connsm, rspbuf); + } + + return ble_ll_cs_start_rsp_make(connsm, rspbuf); +} + +int +ble_ll_cs_rx_cs_start_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr, uint8_t *rspbuf) +{ + int rc; + struct ble_ll_cs_config *conf; + struct ble_ll_cs_proc_params *ps; + uint8_t config_id = *dptr & 0b00111111; + + if (!IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_START) || + CONN_IS_PERIPHERAL(connsm)) { + /* Ignore */ + return BLE_ERR_MAX; + } + + if (config_id != connsm->cssm->config_req_id) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_RSP, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + conf = &connsm->cssm->config[config_id]; + + ps = &conf->proc_params; + ps->anchor_conn_event_cntr = get_le16(dptr + 1); + ps->offset_min = get_le24(dptr + 3); + ps->offset_max = get_le24(dptr + 6); + ps->event_interval = get_le16(dptr + 9); + ps->subevents_per_event = dptr[11]; + ps->subevent_interval = get_le16(dptr + 12); + ps->subevent_len = get_le24(dptr + 14); + ps->aci = dptr[17]; + ps->phy = dptr[18] & 0b00001111; + ps->tx_power_delta = dptr[19]; + + rc = validate_cs_start_parameters(connsm, ps); + if (rc) { + memset(ps, 0, sizeof(*ps)); + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_RSP, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + ps->event_offset = ps->offset_min; + + /* Stop the control procedure and send an event to the host */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_START); + + rc = ble_ll_cs_proc_scheduling_start(connsm, config_id); + if (rc) { + memset(ps, 0, sizeof(*ps)); + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_RSP, rc, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + ble_ll_cs_ev_cs_proc_enable_complete(connsm, config_id, BLE_ERR_SUCCESS); + + return ble_ll_cs_start_ind_make(connsm, rspbuf); +} + +int +ble_ll_cs_rx_cs_start_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf) +{ + int rc; + uint8_t config_id = *dptr & 0b00111111; + struct ble_ll_cs_proc_params *ps; + struct ble_ll_cs_config *conf; + + if (CONN_IS_CENTRAL(connsm)) { + /* Ignore unexpected response */ + return BLE_ERR_MAX; + } + + if (config_id != connsm->cssm->config_req_id) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_IND, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + conf = &connsm->cssm->config[config_id]; + ps = &conf->proc_params; + + /* Overwrite ressugested values */ + ps->anchor_conn_event_cntr = get_le16(dptr + 1); + ps->event_offset = get_le24(dptr + 3); + ps->event_interval = get_le16(dptr + 6); + ps->subevents_per_event = dptr[8]; + ps->subevent_interval = get_le16(dptr + 9); + ps->subevent_len = get_le24(dptr + 11); + ps->aci = dptr[14]; + ps->phy = dptr[15]; + ps->tx_power_delta = dptr[16]; + + rc = validate_cs_start_parameters(connsm, ps); + if (rc) { + memset(ps, 0, sizeof(*ps)); + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_IND, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + rc = ble_ll_cs_proc_scheduling_start(connsm, config_id); + if (rc) { + memset(ps, 0, sizeof(*ps)); + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_IND, rc, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + ble_ll_cs_ev_cs_proc_enable_complete(connsm, config_id, BLE_ERR_SUCCESS); + + return BLE_ERR_MAX; +} + +void +ble_ll_cs_rx_cs_start_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error) +{ + /* Stop the control procedure and send an event to the host */ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_START); + ble_ll_cs_ev_cs_proc_enable_complete(connsm, connsm->cssm->config_req_id, ble_error); +} + +void +ble_ll_cs_terminate_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + struct ble_ll_cs_sm *cssm = connsm->cssm; + uint8_t config_id = cssm->terminate_config_id; + + assert(config_id < ARRAY_SIZE(cssm->config)); + + *dptr = config_id; + put_le16(dptr + 1, cssm->start_procedure_count + cssm->procedure_count); + dptr[3] = cssm->terminate_error_code; +} + +int +ble_ll_cs_rx_cs_terminate_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr, + uint8_t *rspbuf) +{ + struct ble_ll_cs_sm *cssm = connsm->cssm; + uint8_t config_id = *dptr & 0b00111111; + uint16_t procedure_count = get_le16(dptr + 1); + struct ble_ll_cs_proc_params *params; + + if (config_id >= ARRAY_SIZE(cssm->config)) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_TERMINATE_REQ, + BLE_ERR_INV_LMP_LL_PARM, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + params = &cssm->config[config_id].proc_params; + + if (procedure_count > cssm->start_procedure_count + params->max_procedure_count || + procedure_count < cssm->start_procedure_count) { + ble_ll_ctrl_rej_ext_ind_make(BLE_LL_CTRL_CS_TERMINATE_REQ, + BLE_ERR_CMD_DISALLOWED, rspbuf); + return BLE_LL_CTRL_REJECT_IND_EXT; + } + + cssm->terminate_measurement = 1; + + *rspbuf = config_id; + put_le16(rspbuf + 1, cssm->start_procedure_count + cssm->procedure_count); + rspbuf[3] = BLE_ERR_SUCCESS; + + return BLE_LL_CTRL_CS_TERMINATE_RSP; +} + +void +ble_ll_cs_rx_cs_terminate_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr) +{ + struct ble_ll_cs_sm *cssm = connsm->cssm; + uint8_t config_id = *dptr & 0b00111111; + uint8_t error_code = dptr[3]; + uint16_t procedure_count = get_le16(dptr + 1); + uint16_t total_procedure_count; + + if (config_id != cssm->config_req_id || + !IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CS_TERMINATE)) { + return; + } + + total_procedure_count = cssm->start_procedure_count + cssm->procedure_count; + if (total_procedure_count < procedure_count) { + cssm->start_procedure_count = procedure_count; + } else { + cssm->start_procedure_count = total_procedure_count; + } + + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_TERMINATE); + + /* TODO: In d1.0r06 the LL_CS_TERMINATE_IND was replaced with + * LL_CS_TERMINATE_REQ/RSP, but not all descriptions and diagrams + * have been updated yet. + */ + ble_ll_cs_ev_cs_proc_enable_complete(connsm, config_id, error_code); +} + +void +ble_ll_cs_rx_cs_terminate_req_rejected(struct ble_ll_conn_sm *connsm, uint8_t ble_error) +{ + ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CS_TERMINATE); + ble_ll_cs_ev_cs_proc_enable_complete(connsm, connsm->cssm->config_req_id, ble_error); } int @@ -116,4 +1849,66 @@ ble_ll_cs_hci_test_end(void) return BLE_ERR_UNSUPPORTED; } +void +ble_ll_cs_init(void) +{ + struct ble_ll_cs_supp_cap *cap = &g_ble_ll_cs_local_cap; + + /* Set local CS capabilities. Only basic features supported for now. */ + cap->mode_types = 0x00; + cap->rtt_capability = 0x00; + cap->rtt_aa_only_n = 0x00; + cap->rtt_sounding_n = 0x00; + cap->rtt_random_sequence_n = 0x00; + cap->nadm_sounding_capability = 0x0000; + cap->nadm_random_sequence_capability = 0x0000; + cap->cs_sync_phy_capability = 0x00; + cap->number_of_antennas = 0x01; + cap->max_number_of_antenna_paths = 0x01; + cap->roles_supported = 0x03; + cap->no_fae = 0x00; + cap->channel_selection = 0x00; + cap->sounding_pct_estimate = 0x00; + cap->max_number_of_configs = 0x04; + cap->max_number_of_procedures = 0x0001; + cap->t_sw = 0x0A; + cap->t_ip1_capability = 1 << T_IP1_CAP_ID_145US; + cap->t_ip2_capability = 1 << T_IP2_CAP_ID_145US; + cap->t_fcs_capability = 1 << T_FCS_CAP_ID_150US; + cap->t_pm_capability = 1 << T_PM_CAP_ID_40US; + cap->tx_snr_capablity = 0x00; + + ble_ll_cs_proc_set_chan_class(default_channel_classification); +} + +void +ble_ll_cs_reset(void) +{ + ble_ll_cs_init(); +} + +void +ble_ll_cs_sm_init(struct ble_ll_conn_sm *connsm) +{ + uint8_t i; + + for (i = 0; i < ARRAY_SIZE(g_ble_ll_cs_sm); i++) { + if (g_ble_ll_cs_sm[i].connsm == NULL) { + connsm->cssm = &g_ble_ll_cs_sm[i]; + memset(connsm->cssm, 0, sizeof(*connsm->cssm)); + connsm->cssm->connsm = connsm; + break; + } + } +} + +void +ble_ll_cs_sm_free(struct ble_ll_conn_sm *connsm) +{ + if (connsm->cssm) { + memset(connsm->cssm, 0, sizeof(*connsm->cssm)); + connsm->cssm = NULL; + } +} + #endif /* BLE_LL_CHANNEL_SOUNDING */ diff --git a/nimble/controller/src/ble_ll_cs_drbg.c b/nimble/controller/src/ble_ll_cs_drbg.c new file mode 100644 index 0000000000..7de5aab340 --- /dev/null +++ b/nimble/controller/src/ble_ll_cs_drbg.c @@ -0,0 +1,689 @@ +/* + * 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 +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include +#include +#include +#include "nimble/ble.h" +#include "mbedtls/aes.h" +#include "ble_ll_cs_drbg_priv.h" +#if !(BABBLESIM || MYNEWT_VAL(SELFTEST)) +#include "controller/ble_hw.h" +#endif + +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); + } else { + rc = -1; + } + + return rc; +} + +/** + * DRBG chain function f7. + * - k - 128-bit key + * - in - an input bit string whose length is a multiple of 128 bits and + * generates an output that is 128 bits long using a cipher block + * chaining technique. + * - out - processed bit string + */ +int +ble_ll_cs_drbg_f7(const uint8_t *k, const uint8_t *in, uint8_t len, uint8_t *out) +{ + int rc = 0; + int i; + const uint64_t *block; + uint64_t *hout = (uint64_t *)out; + + memset(hout, 0, 16); + + /* Starting with the leftmost bits (MSO) of input_bit_string, split into + * 128-bit blocks + */ + for (i = len - 16; i >= 0; i -= 16) { + block = (uint64_t *)&in[i]; + /* XOR a 128-bit block in two steps */ + *hout ^= *block; + *(hout + 1) ^= *(block + 1); + + rc = ble_ll_cs_drbg_e(k, out, out); + if (rc) { + break; + } + } + + return rc; +} + +/** + * DRBG derivation function f8. + * - input - 320-bit input bit string + * - sm - output, generated 256-bit seed material (SM) + */ +int +ble_ll_cs_drbg_f8(const uint8_t *input, uint8_t *sm) +{ + int rc; + uint8_t k[16] = {0x0F, 0x0E, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x08, + 0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00}; + uint8_t k2[16] = {0}; + uint8_t x[16] = {0}; + /* buf contains V || S */ + uint8_t buf[80] = {0}; + uint8_t *s = buf; + uint8_t *v = buf + 64; + + /* S = 0x0000002800000020 || input_bit_string || 0x80 || + * 0x000000000000000000000000000000 + */ + s[15] = 0x80; + memcpy(s + 16, input, 40); + put_le64(s + 56, 0x0000002800000020); + + /* K2 = f7( K, V || S ) */ + rc = ble_ll_cs_drbg_f7(k, buf, 80, k2); + if (rc) { + return rc; + } + + /* V = 0x00000001000000000000000000000000 */ + v[12] = 0x01; + + /* X = f7( K, V || S ) */ + rc = ble_ll_cs_drbg_f7(k, buf, 80, x); + if (rc) { + return rc; + } + + /* Calculate the most significant part of SM: + * SM = e( K2, X ) + */ + rc = ble_ll_cs_drbg_e(k2, x, sm + 16); + if (rc) { + return rc; + } + + /* Calculate the least significant part of SM and concatenate + * both parts: + * SM = SM || e( K2, SM ) + */ + rc = ble_ll_cs_drbg_e(k2, sm + 16, sm); + + return rc; +} + +/** + * DRBG update function f9 is used to update and refresh a DRBG 128-bit + * temporal key K and a 128-bit nonce vector V using a 256-bit seed material (SM) + * that may carry fresh entropy. The SM value may also be 0 if f9 is called for + * backtracking purposes. + * - sm - 256-bit seed material + * - k_in - 128-bit key + * - v_in - 128-bit nonce vector + */ +int +ble_ll_cs_drbg_f9(const uint8_t *sm, uint8_t *k, uint8_t *v) +{ + int rc; + uint8_t x[32] = {0}; + uint64_t *x_p = (uint64_t *)x; + uint64_t *sm_p = (uint64_t *)sm; + + /* V = V[127:8] || (( V[7:0] + 1 ) mod 2^8) */ + v[0]++; + rc = ble_ll_cs_drbg_e(k, v, x + 16); + if (rc) { + return rc; + } + + v[0]++; + /* Again V = V[127:8] || (( V[7:0] + 1 ) mod 2^8) */ + rc = ble_ll_cs_drbg_e(k, v, x); + if (rc) { + return rc; + } + + /* X = X ⊕ SM */ + x_p[0] ^= sm_p[0]; + x_p[1] ^= sm_p[1]; + x_p[2] ^= sm_p[2]; + x_p[3] ^= sm_p[3]; + + memcpy(v, x, 16); + memcpy(k, x + 16, 16); + + return 0; +} + +/** + * DRBG instantiation function h9. + * - iv - 128-bit initialization vector (CS_IV) + * - in - 64-bit instantiation nonce (CS_IN) + * - pv - 128-bit personalization vector (CS_PV) + * - key - output, 128-bit temporal key (K_DRBG) + * - nonce_v - output, 128-bit nonce vector (V_DRBG) + */ +int +ble_ll_cs_drbg_h9(const uint8_t *iv, const uint8_t *in, const uint8_t *pv, + uint8_t *key, uint8_t *nonce_v) +{ + int rc; + uint8_t input_bit_string[40] = {0}; + uint8_t sm[32] = {0}; + + /* 320-bit input bit string created from concatenated vectors + * CS_IV || CS_IN || CS_PV + */ + memcpy(input_bit_string, pv, 16); + memcpy(input_bit_string + 16, in, 8); + memcpy(input_bit_string + 24, iv, 16); + + /* Generate seed material (SM) */ + rc = ble_ll_cs_drbg_f8(input_bit_string, sm); + if (rc) { + return rc; + } + + /* Generate K_DRBG and V_DRBG */ + memset(key, 0, 16); + memset(nonce_v, 0, 16); + rc = ble_ll_cs_drbg_f9(sm, key, nonce_v); + + return rc; +} + +/** + * Random bit generation function CS_DRBG + * - transaction_id - CSTransactionID, + * - drbg_ctx - the drbg context, already inited with keys, + * - output - output buffer, + * - len - number of bytes to be generated. + */ +int +ble_ll_cs_drbg_rand(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t transaction_id, + uint8_t *output, uint8_t len) +{ + int rc; + uint8_t rand_len = 0; + uint8_t nonce[16]; + struct ble_ll_cs_transaction_cache *cache = &drbg_ctx->t_cache[transaction_id]; + + /* Set the fixed values of the DRGB nonce */ + memcpy(nonce, drbg_ctx->nonce_v, sizeof(nonce)); + /* Set the Transaction_Counter */ + if (cache->last_step_count != step_count) { + cache->transaction_counter = 0; + cache->last_step_count = step_count; + } + nonce[0] += cache->transaction_counter; + /* Set the Transaction_Identifier */ + nonce[1] += transaction_id; + /* Set the CS Step_Counter */ + put_le16(&nonce[2], step_count + get_le16(&nonce[2])); + + while (len > 0) { + if (cache->free_bytes > 0) { + /* Use bytes from previous DRBG invocation */ + + if (len < cache->free_bytes) { + rand_len = len; + } else { + rand_len = cache->free_bytes; + } + + cache->free_bytes -= rand_len; + /* [0] is LSO, [15] is MSO. Return cached bytes starting from MSO. */ + memcpy(output, cache->random_bytes + cache->free_bytes, rand_len); + + len -= rand_len; + output += rand_len; + } else { + /* Invoke CS_DRBG to get fresh 128-bit sequence */ + rc = ble_ll_cs_drbg_e(drbg_ctx->key, nonce, cache->random_bytes); + if (rc) { + return rc; + } + + cache->free_bytes = sizeof(cache->random_bytes); + /* Increment the transaction counter */ + ++nonce[0]; + ++cache->transaction_counter; + } + } + + return 0; +} + +/** Channel Sounding random number generation function hr1 */ +int +ble_ll_cs_drbg_rand_hr1(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count, + uint8_t transaction_id, uint8_t r, uint8_t *r_out) +{ + int rc; + uint16_t t_rand; + uint8_t random_bits; + + if (r <= 1) { + *r_out = 0; + + return 0; + } + + rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count, + transaction_id, &random_bits, 1); + if (rc) { + return rc; + } + + t_rand = r * random_bits; + + if ((t_rand & 0xFF) < (256 % r)) { + rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count, + transaction_id, &random_bits, 1); + *r_out = ((256 * random_bits * r) + t_rand) / 65536; + } else { + *r_out = t_rand / 256; + } + + return rc; +} + +int +ble_ll_cs_drbg_shuffle_cr1(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count, + uint8_t transaction_id, uint8_t *channel_array, + uint8_t *shuffled_array, uint8_t len) +{ + int rc; + uint8_t i, j; + + for (i = 0; i < len; ++i) { + rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count, transaction_id, i + 1, &j); + if (rc) { + return rc; + } + + if (i != j) { + shuffled_array[i] = shuffled_array[j]; + } + + shuffled_array[j] = channel_array[i]; + } + + return 0; +} + +static int +cs_autocorrelation_score(uint32_t sequence) +{ + int i, k, score = 0; + uint32_t c, s; + + for (k = 1; k <= 3; ++k) { + c = 0; + s = sequence; + for (i = 1; i <= 32 - k; ++i) { + c += (s & 1) ^ ((s >> k) & 1); + s >>= 1; + } + score += abs(2 * c - (32 - k)); + } + + return score; +} + +int +ble_ll_cs_drbg_generate_aa(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count, + uint32_t *initiator_aa, uint32_t *reflector_aa) +{ + int rc; + uint8_t buf[16]; + uint32_t s0, s1, s2, s3; + + rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count, + BLE_LL_CS_DRBG_ACCESS_ADDRESS, + buf, sizeof(buf)); + + if (rc) { + return rc; + } + + /* The buf[15] is the first generated octet */ + s0 = get_le32(&buf[12]); + s1 = get_le32(&buf[8]); + s2 = get_le32(&buf[4]); + s3 = get_le32(&buf[0]); + + /* The sequence with the lower autocorrelation score is selected + * as the CS Access Address. See 2.2.1 Channel Sounding Access Address + * selection rules. + */ + if (cs_autocorrelation_score(s0) < cs_autocorrelation_score(s1)) { + *initiator_aa = s0; + } else { + *initiator_aa = s1; + } + + if (cs_autocorrelation_score(s2) < cs_autocorrelation_score(s3)) { + *reflector_aa = s2; + } else { + *reflector_aa = s3; + } + + return 0; +} + +int +ble_ll_cs_drbg_rand_marker_position(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t rtt_type, + uint8_t *position1, uint8_t *position2) +{ + int rc; + uint8_t rand_range; + + if (rtt_type == BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE) { + rand_range = 29; + } else { /* BLE_LL_CS_RTT_96_BIT_SOUNDING_SEQUENCE */ + rand_range = 64; + } + + rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count, + BLE_LL_CS_DRBG_SEQ_MARKER_POSITION, + rand_range, position1); + if (rc) { + return -1; + } + + if (rtt_type == BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE) { + *position2 = 0xFF; + + return 0; + } + + rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count, + BLE_LL_CS_DRBG_SEQ_MARKER_POSITION, + 75, position2); + if (rc) { + return -1; + } + + *position2 += 67; + if (*position2 > 92) { + /* Omit the second marker */ + *position2 = 0xFF; + } + + return 0; +} + +int +ble_ll_cs_drbg_rand_marker_selection(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, uint8_t *marker_selection) +{ + int rc; + + if (drbg_ctx->marker_selection_free_bits == 0) { + rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count, + BLE_LL_CS_DRBG_SEQ_MARKER_SIGNAL, + &drbg_ctx->marker_selection_cache, 1); + if (rc) { + return rc; + } + + drbg_ctx->marker_selection_free_bits = 8; + } + + *marker_selection = drbg_ctx->marker_selection_cache & 0x80; + drbg_ctx->marker_selection_cache <<= 1; + --drbg_ctx->marker_selection_free_bits; + + return 0; +} + +int +ble_ll_cs_drbg_rand_main_mode_steps(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, + uint8_t main_mode_min_steps, + uint8_t main_mode_max_steps, + uint8_t *main_mode_steps) +{ + int rc; + uint8_t r; + uint8_t r_out; + + r = main_mode_max_steps - main_mode_min_steps + 1; + rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count, + BLE_LL_CS_DRBG_SUBEVT_SUBMODE, + r, &r_out); + if (rc) { + return rc; + } + + *main_mode_steps = r_out + main_mode_min_steps; + + return 0; +} + +static int +ble_ll_cs_drbg_apply_marker_signal(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint8_t step_count, + uint8_t *buf, uint8_t position) +{ + int rc; + uint16_t *byte_ptr; + uint16_t marker_signal; + uint8_t marker_selection; + uint8_t byte_id = 0; + uint8_t bit_offset = 0; + + rc = ble_ll_cs_drbg_rand_marker_selection(drbg_ctx, step_count, &marker_selection); + if (rc) { + return rc; + } + + if (marker_selection) { + /* '0011' in transmission order */ + marker_signal = 0b1100; + } else { + /* '1100' in transmission order */ + marker_signal = 0b0011; + } + + byte_id = position / 8; + byte_ptr = (uint16_t *) &buf[byte_id]; + bit_offset = position % 8; + *byte_ptr &= ~(0xF << bit_offset); + *byte_ptr |= ~(marker_signal << bit_offset); + + return 0; +} + +static int +ble_ll_cs_generate_sounding_sequence(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, uint8_t rtt_type, + uint8_t *buf, uint8_t sequence_len) +{ + int rc; + uint8_t i; + uint8_t position1; + uint8_t position2; + + for (i = 0; i < sequence_len; ++i) { + buf[i] = 0b10101010; + } + + rc = ble_ll_cs_drbg_rand_marker_position(drbg_ctx, step_count, rtt_type, + &position1, &position2); + if (rc) { + return rc; + } + + rc = ble_ll_cs_drbg_apply_marker_signal(drbg_ctx, step_count, buf, position1); + if (rc) { + return rc; + } + + if (position2 != 0xFF) { + rc = ble_ll_cs_drbg_apply_marker_signal(drbg_ctx, step_count, buf, position2); + } + + return rc; +} + +int +ble_ll_cs_drbg_generate_sync_sequence(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, + uint8_t rtt_type, + uint8_t *buf, + uint8_t *sequence_len) +{ + int rc = -1; + + *sequence_len = rtt_seq_len[rtt_type]; + + switch (rtt_type) { + case BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE: + rc = ble_ll_cs_generate_sounding_sequence(drbg_ctx, step_count, rtt_type, + buf, *sequence_len); + break; + case BLE_LL_CS_RTT_96_BIT_SOUNDING_SEQUENCE: + rc = ble_ll_cs_generate_sounding_sequence(drbg_ctx, step_count, rtt_type, + buf, *sequence_len); + break; + case BLE_LL_CS_RTT_32_BIT_RANDOM_SEQUENCE: + case BLE_LL_CS_RTT_64_BIT_RANDOM_SEQUENCE: + case BLE_LL_CS_RTT_96_BIT_RANDOM_SEQUENCE: + case BLE_LL_CS_RTT_128_BIT_RANDOM_SEQUENCE: + rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count, + BLE_LL_CS_DRBG_RAND_SEQ_GENERATION, + buf, *sequence_len); + break; + default: + break; + } + + return rc; +} + +int +ble_ll_cs_drbg_rand_tone_ext_presence(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, uint8_t *presence) +{ + int rc; + + if (drbg_ctx->tone_ext_presence_free_bits == 0) { + rc = ble_ll_cs_drbg_rand(drbg_ctx, step_count, + BLE_LL_CS_DRBG_T_PM_TONE_SLOT_PRESENCE, + &drbg_ctx->tone_ext_presence_cache, 1); + if (rc) { + return rc; + } + + drbg_ctx->tone_ext_presence_free_bits = 8; + } + + *presence = drbg_ctx->tone_ext_presence_cache & 0x80 ? 1 : 0; + drbg_ctx->tone_ext_presence_cache <<= 1; + --drbg_ctx->tone_ext_presence_free_bits; + + return 0; +} + +int +ble_ll_cs_drbg_rand_antenna_path_perm_id(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t n_ap, + uint8_t *ap_id) +{ + int rc; + uint8_t i; + uint8_t n_ap_f = 0; + + if (n_ap <= 1) { + *ap_id = 0; + + return 0; + } + + assert(n_ap <= 4); + + for (i = 1; i <= n_ap; ++i) { + n_ap_f *= i; + } + + rc = ble_ll_cs_drbg_rand_hr1(drbg_ctx, step_count, + BLE_LL_CS_DRBG_ANTENNA_PATH_PERMUTATION, + n_ap_f, ap_id); + + return rc; +} + +void +ble_ll_cs_drbg_clear_cache(struct ble_ll_cs_drbg_ctx *drbg_ctx) +{ + memset(drbg_ctx->t_cache, 0, sizeof(drbg_ctx->t_cache)); + drbg_ctx->marker_selection_cache = 0; + drbg_ctx->marker_selection_free_bits = 0; +} + +int +ble_ll_cs_drbg_init(struct ble_ll_cs_drbg_ctx *drbg_ctx) +{ + /* Calculate temporal key K and nonce vector V */ + return ble_ll_cs_drbg_h9(drbg_ctx->iv, drbg_ctx->in, drbg_ctx->pv, + drbg_ctx->key, drbg_ctx->nonce_v); +} + +void +ble_ll_cs_drbg_free(struct ble_ll_cs_drbg_ctx *drbg_ctx) +{ + memset(drbg_ctx, 0, sizeof(*drbg_ctx)); +} +#endif /* BLE_LL_CHANNEL_SOUNDING */ diff --git a/nimble/controller/src/ble_ll_cs_drbg_priv.h b/nimble/controller/src/ble_ll_cs_drbg_priv.h new file mode 100644 index 0000000000..6e49e06789 --- /dev/null +++ b/nimble/controller/src/ble_ll_cs_drbg_priv.h @@ -0,0 +1,129 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_CS_DRBG_PRIV +#define H_BLE_LL_CS_DRBG_PRIV + +#ifdef __cplusplus +extern "C" { +#endif + +#define BLE_LL_CS_DRBG_HOP_CHAN_NON_MODE0 (0x00) +#define BLE_LL_CS_DRBG_HOP_CHAN_MODE0 (0x01) +#define BLE_LL_CS_DRBG_SUBEVT_SUBMODE (0x02) +#define BLE_LL_CS_DRBG_T_PM_TONE_SLOT_PRESENCE (0x03) +#define BLE_LL_CS_DRBG_ANTENNA_PATH_PERMUTATION (0x04) +#define BLE_LL_CS_DRBG_ACCESS_ADDRESS (0x05) +#define BLE_LL_CS_DRBG_SEQ_MARKER_POSITION (0x06) +#define BLE_LL_CS_DRBG_SEQ_MARKER_SIGNAL (0x07) +#define BLE_LL_CS_DRBG_RAND_SEQ_GENERATION (0x08) +#define BLE_LL_CS_DRBG_BACKTRACKING_RESISTANCE (0x09) +#define BLE_LL_CS_DRBG_TRANSACTION_IDS_NUMBER (0x0a) + +#define BLE_LL_CS_RTT_AA_ONLY (0x00) +#define BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE (0x01) +#define BLE_LL_CS_RTT_96_BIT_SOUNDING_SEQUENCE (0x02) +#define BLE_LL_CS_RTT_32_BIT_RANDOM_SEQUENCE (0x03) +#define BLE_LL_CS_RTT_64_BIT_RANDOM_SEQUENCE (0x04) +#define BLE_LL_CS_RTT_96_BIT_RANDOM_SEQUENCE (0x05) +#define BLE_LL_CS_RTT_128_BIT_RANDOM_SEQUENCE (0x06) + +struct ble_ll_cs_transaction_cache { + uint16_t last_step_count; + /* 1-octet CSTransactionCounter per each CS Transaction ID. Should + * be reset each time the nonce V CS Step_Counter field is set to + * a new value. + */ + uint8_t transaction_counter; + /* Random bits cache */ + uint8_t random_bytes[16]; + /* The number of cached bytes that have been already used */ + uint8_t free_bytes; +}; + +/* DRBG context */ +struct ble_ll_cs_drbg_ctx { + /* Initialization vector, entropy input */ + uint8_t iv[16]; + /* Instantiation nonce */ + uint8_t in[8]; + /* Personalization vector/string */ + uint8_t pv[16]; + /* Temporal key K */ + uint8_t key[16]; + /* DRBG nonce/counter (V), the starting value from which the DRBG operates. + * Initialized once per LE Connection. + */ + uint8_t nonce_v[16]; + /* Cache bits generated with single DRBG transation */ + struct ble_ll_cs_transaction_cache t_cache[BLE_LL_CS_DRBG_TRANSACTION_IDS_NUMBER]; + + uint8_t marker_selection_free_bits; + uint8_t marker_selection_cache; + + uint8_t tone_ext_presence_free_bits; + uint8_t tone_ext_presence_cache; +}; + +int ble_ll_cs_drbg_e(const uint8_t *key, const uint8_t *data, uint8_t *out_data); +int ble_ll_cs_drbg_f7(const uint8_t *k, const uint8_t *in, uint8_t len, uint8_t *out); +int ble_ll_cs_drbg_f8(const uint8_t *input, uint8_t *sm); +int ble_ll_cs_drbg_f9(const uint8_t *sm, uint8_t *k, uint8_t *v); +int ble_ll_cs_drbg_h9(const uint8_t *iv, const uint8_t *in, const uint8_t *pv, + uint8_t *key, uint8_t *nonce_v); +int ble_ll_cs_drbg_rand(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t transaction_id, + uint8_t *output, uint8_t len); +int ble_ll_cs_drbg_rand_hr1(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t transaction_id, + uint8_t r, uint8_t *r_out); +int ble_ll_cs_drbg_shuffle_cr1(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count, + uint8_t transaction_id, uint8_t *channel_array, + uint8_t *shuffled_array, uint8_t len); +int ble_ll_cs_drbg_generate_aa(struct ble_ll_cs_drbg_ctx *drbg_ctx, uint16_t step_count, + uint32_t *initiator_aa, uint32_t *reflector_aa); +int ble_ll_cs_drbg_rand_marker_position(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t rtt_type, + uint8_t *position1, uint8_t *position2); +int ble_ll_cs_drbg_rand_marker_selection(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, uint8_t *marker_selection); +int ble_ll_cs_drbg_rand_main_mode_steps(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, + uint8_t main_mode_min_steps, + uint8_t main_mode_max_steps, + uint8_t *main_mode_steps); +int ble_ll_cs_drbg_generate_sync_sequence(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, + uint8_t rtt_type, + uint8_t *buf, + uint8_t *sequence_len); +int ble_ll_cs_drbg_rand_antenna_path_perm_id(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint16_t step_count, uint8_t n_ap, + uint8_t *ap_id); +int ble_ll_cs_drbg_rand_tone_ext_presence(struct ble_ll_cs_drbg_ctx *drbg_ctx, + uint8_t step_count, uint8_t *presence); +int ble_ll_cs_drbg_init(struct ble_ll_cs_drbg_ctx *drbg_ctx); +void ble_ll_cs_drbg_free(struct ble_ll_cs_drbg_ctx *drbg_ctx); +void ble_ll_cs_drbg_clear_cache(struct ble_ll_cs_drbg_ctx *drbg_ctx); + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_CS_DRBG_PRIV */ diff --git a/nimble/controller/src/ble_ll_cs_priv.h b/nimble/controller/src/ble_ll_cs_priv.h new file mode 100644 index 0000000000..41938e3d27 --- /dev/null +++ b/nimble/controller/src/ble_ll_cs_priv.h @@ -0,0 +1,349 @@ +/* + * 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. + */ + +#ifndef H_BLE_LL_CS_PRIV +#define H_BLE_LL_CS_PRIV + +#include +#include "controller/ble_ll_conn.h" +#include "ble_ll_cs_drbg_priv.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define BLE_LL_CS_MODE0 (0) +#define BLE_LL_CS_MODE1 (1) +#define BLE_LL_CS_MODE2 (2) +#define BLE_LL_CS_MODE3 (3) + +/* States within step */ +#define STEP_STATE_INIT (0) +#define STEP_STATE_CS_SYNC_I (1) +#define STEP_STATE_CS_SYNC_R (2) +#define STEP_STATE_CS_TONE_I (3) +#define STEP_STATE_CS_TONE_R (4) +#define STEP_STATE_COMPLETE (5) + +#define BLE_LL_CS_ROLE_INITIATOR (0) +#define BLE_LL_CS_ROLE_REFLECTOR (1) + +#define BLE_LL_CS_CONFIG_MAX_NUM 4 +/* CS Event interval in units of the number of connection intervals */ +#define BLE_LL_CS_EVENT_INTERVAL_MIN (0x0001) +#define BLE_LL_CS_EVENT_INTERVAL_MAX (0xFFFF) +#define BLE_LL_CS_SUBEVENTS_PER_PROCEDURE_MIN (1) +#define BLE_LL_CS_SUBEVENTS_PER_PROCEDURE_MAX (32) +/* CS Subevent interval in units of 625usec */ +#define BLE_LL_CS_SUBEVENTS_INTERVAL_MIN (1) +#define BLE_LL_CS_SUBEVENTS_INTERVAL_MAX (32) +/* CS Subevent interval in microseconds */ +#define BLE_LL_CS_SUBEVENT_LEN_MIN (1250) +#define BLE_LL_CS_SUBEVENT_LEN_MAX (4000000) + +/* CS Event interval in units of the number of connection intervals */ +#define BLE_LL_CS_SUBEVENTS_INTERVAL_UNIT_US (625) +#define BLE_LL_CS_PROCEDURE_LEN_UNIT_US (625) + +#define BLE_LL_CS_STEPS_PER_SUBEVENT_MIN (2) +#define BLE_LL_CS_STEPS_PER_SUBEVENT_MAX (160) +#define BLE_LL_CS_STEPS_PER_PROCEDURE_MAX (256) + +#define BLE_LL_CS_SYNC_PHY_1M (0x01) +#define BLE_LL_CS_SYNC_PHY_2M (0x02) +/* The duration of the CS_SYNC (T_SY) without sequence in usec */ +#define BLE_LL_CS_SYNC_TIME_1M (44) +#define BLE_LL_CS_SYNC_TIME_2M (26) + +typedef int (*ble_ll_cs_sched_cb_func)(struct ble_ll_cs_sm *cssm); + +struct ble_ll_cs_step_transmission { + struct ble_phy_cs_transmission phy_transm; + uint8_t state; +}; + +struct ble_ll_cs_step { + uint8_t mode; + uint8_t channel; + uint8_t tone_ext_presence_i; + uint8_t tone_ext_presence_r; + uint32_t initiator_aa; + uint32_t reflector_aa; + struct ble_ll_cs_step_transmission *next_transm; + struct ble_ll_cs_step_transmission *last_transm; + uint8_t *rtt_tx; + uint8_t *rtt_rx; + uint8_t rtt_tx_len; + uint8_t rtt_rx_len; +}; + +struct ble_ll_cs_aci { + uint8_t n_ap; + uint8_t n_a_antennas; + uint8_t n_b_antennas; +}; + +struct ble_ll_cs_supp_cap { + uint8_t mode_types; + uint8_t roles_supported; + uint8_t rtt_capability; + uint8_t rtt_aa_only_n; + uint8_t rtt_sounding_n; + uint8_t rtt_random_sequence_n; + uint16_t nadm_sounding_capability; + uint16_t nadm_random_sequence_capability; + uint8_t cs_sync_phy_capability; + uint8_t number_of_antennas; + uint8_t max_number_of_antenna_paths; + uint8_t no_fae; + uint8_t channel_selection; + uint8_t sounding_pct_estimate; + uint8_t max_number_of_configs; + uint16_t max_number_of_procedures; + uint16_t t_ip1_capability; + uint16_t t_ip2_capability; + uint16_t t_fcs_capability; + uint16_t t_pm_capability; + uint8_t t_sw; + uint8_t tx_snr_capablity; +}; + +struct ble_ll_cs_pref_proc_params { + uint16_t max_procedure_len; + uint16_t min_procedure_interval; + uint16_t max_procedure_interval; + uint16_t max_procedure_count; + uint32_t min_subevent_len; + uint32_t max_subevent_len; + uint8_t aci; + uint8_t phy; + uint8_t tx_power_delta; + uint8_t preferred_peer_antenna; + uint8_t snr_control_initiator; + uint8_t snr_control_reflector; + uint8_t params_ready; +}; + +/* Negotiated parameters for CS procedures */ +struct ble_ll_cs_proc_params { + /* The number of consecutive CS procedures to invoke */ + uint16_t max_procedure_count; + /* The event counter that will be the anchor point to + * the start of the first CS procedure. + */ + uint16_t anchor_conn_event_cntr; + /* The offset (usec) from the event counter to the CS subevent anchor point */ + uint32_t event_offset; + uint32_t offset_min; + uint32_t offset_max; + /* Max CS procedure length in 0.625us units */ + uint16_t max_procedure_len; + /* Max CS subevent length in microseconds */ + uint32_t subevent_len; + uint16_t procedure_interval; + uint16_t event_interval; + uint16_t subevent_interval; + uint8_t subevents_per_event; + /* Selected Anntena Configuration ID */ + uint8_t aci; + /* Bit map of preferred peer-ordered antenna elements */ + uint8_t preferred_peer_antenna; + /* Bit map of the remote device’s Tx PHY */ + uint8_t phy; + /* Transmit power delta, in signed dB, to indicate the recommended difference + * between the remote device’s power level for the CS tones and RTT packets + * and the existing power level for the PHY indicated by the PHY parameter. + */ + uint8_t tx_power_delta; + /* SNR Output Index (SOI) for SNR control adjustment */ + uint8_t tx_snr_i; + uint8_t tx_snr_r; +}; + +struct ble_ll_cs_config { + uint8_t config_in_use; + uint8_t config_enabled; + /* The role to use in CS procedure + * 0x00 = Initiator, + * 0x01 = Reflector + */ + uint8_t role; + /* Map of allowed channels for this CS config */ + uint8_t chan_map[10]; + /* The number of times the map represented by the Channel_Map field is to + * be cycled through for non-mode 0 steps within a CS procedure + */ + uint8_t chan_map_repetition; + uint8_t main_mode; + uint8_t sub_mode; + uint8_t main_mode_min_steps; + uint8_t main_mode_max_steps; + uint8_t main_mode_repetition; + uint8_t mode_0_steps; + /* PHY used for mode 0, 1 and 3 (use LE 1M PHY)*/ + uint8_t cs_sync_phy; + /* Type of RTT (Round-Trip Time) packets */ + uint8_t rtt_type; + /* The Channel Selection Algorithm to use */ + uint8_t chan_sel; + /* Parameters for #3c algorithm */ + uint8_t ch3cshape; + uint8_t ch3cjump; + /* Timings (indexes) selected from capabilities */ + uint8_t t_ip1_index; + uint8_t t_ip2_index; + uint8_t t_fcs_index; + uint8_t t_pm_index; + /* Timings (usec) selected from capabilities */ + uint8_t t_ip1; + uint8_t t_ip2; + uint8_t t_fcs; + uint8_t t_pm; + /* CS procedure parameters preferred by our Host */ + struct ble_ll_cs_pref_proc_params pref_proc_params; + /* Final negationed CS procedure params */ + struct ble_ll_cs_proc_params proc_params; +}; + +struct ble_ll_cs_step_result { + uint8_t sounding_pct_estimate; + uint8_t packet_rssi; + uint8_t packet_quality; + uint8_t packet_nadm; + uint32_t time_of_departure_us; + uint32_t time_of_departure_ns; + uint32_t time_of_arrival_us; + uint32_t time_of_arrival_ns; + uint32_t packet_pct1; + uint32_t packet_pct2; + uint16_t measured_freq_offset; + uint32_t tone_pct[5]; + uint8_t tone_quality_ind[5]; +}; + +struct ble_ll_cs_subevent { + struct ble_hci_ev *hci_ev; + unsigned int subev; + uint8_t num_steps_reported; +}; + +struct ble_ll_cs_sm { + struct ble_ll_conn_sm *connsm; + struct ble_ll_cs_supp_cap remote_cap; + struct ble_ll_cs_config config[BLE_LL_CS_CONFIG_MAX_NUM]; + + /* Default Settings */ + uint8_t roles_enabled; + uint8_t cs_sync_antenna_selection; + uint8_t max_tx_power; + + /* Cached FAE tables */ + uint8_t remote_fae_table[72]; + uint8_t local_fae_table[72]; + + struct ble_ll_cs_config *active_config; + uint8_t active_config_id; + + /* Arguments for ble_ll_cs_config_req_make */ + uint8_t config_req_id; + uint8_t config_req_action; + struct ble_ll_cs_config tmp_config; + + /* Arguments for ble_ll_cs_hci_proc_enable */ + uint8_t terminate_config_id; + uint8_t terminate_error_code; + + /* DRBG context, initialized onece per LE Connection */ + struct ble_ll_cs_drbg_ctx drbg_ctx; + + /* Helper flags */ + uint8_t measurement_enabled; + uint8_t terminate_measurement; + + /* Scheduling data for current CS procedure */ + struct ble_ll_sched_item sch; + ble_ll_cs_sched_cb_func sched_cb; + uint32_t anchor_usecs; + struct ble_ll_cs_step *current_step; + struct ble_ll_cs_step *last_step; + uint8_t *next_rtt_ptr; + /* Cached main mode channels that will be used in repetition steps */ + uint8_t repetition_channels[3]; + uint8_t n_ap; + + /* Counters of complete CS procedures/events/subevents/steps */ + uint16_t procedure_count; + uint16_t events_in_procedure_count; + uint16_t subevents_in_procedure_count; + uint16_t subevents_in_event_count; + uint16_t steps_in_procedure_count; + uint8_t steps_in_subevent_count; + + /* Down-counters of remaining steps */ + uint8_t mode0_step_count; + uint8_t repetition_count; + uint8_t main_step_count; + + /* Anchor time of current CS procedure */ + uint32_t procedure_anchor_usecs; + /* Anchor time of current CS event */ + uint32_t event_anchor_usecs; + /* Anchor time of current CS subevent */ + uint32_t subevent_anchor_usecs; + /* Anchor time of next CS step */ + uint32_t step_anchor_usecs; + uint32_t event_interval_usecs; + uint32_t subevent_interval_usecs; + uint32_t procedure_interval_usecs; + /* Estimated time of step modes (ToF not included) */ + uint32_t mode_duration_usecs[4]; + /* Time of antenna swith */ + uint8_t t_sw; + /* Time of CS_SYNC packet without sequence */ + uint8_t t_sy; + /* Time of CS_SYNC sequence only */ + uint8_t t_sy_seq; + uint8_t cs_sync_antenna; + + /* Cache for HCI Subevent Result event */ + struct ble_ll_cs_subevent buffered_subevent; + struct ble_ll_cs_step_result step_result; + uint8_t cs_schedule_status; + uint8_t proc_abort_reason; + uint8_t subev_abort_reason; + + /* Channel selection stuff */ + uint8_t mode0_channels[72]; + uint8_t non_mode0_channels[72]; + uint8_t mode0_next_chan_id; + uint8_t non_mode0_next_chan_id; + + /* Procedure counter for backtracking resistance */ + uint16_t start_procedure_count; +}; + +int ble_ll_cs_proc_schedule_next_tx_or_rx(struct ble_ll_cs_sm *cssm); +int ble_ll_cs_proc_scheduling_start(struct ble_ll_conn_sm *connsm, uint8_t config_id); +void ble_ll_cs_proc_sync_lost(struct ble_ll_cs_sm *cssm); + +#ifdef __cplusplus +} +#endif + +#endif /* H_BLE_LL_CS_PRIV */ diff --git a/nimble/controller/src/ble_ll_cs_proc.c b/nimble/controller/src/ble_ll_cs_proc.c new file mode 100644 index 0000000000..9d921c7b1b --- /dev/null +++ b/nimble/controller/src/ble_ll_cs_proc.c @@ -0,0 +1,1530 @@ +/* + * 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 +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include +#include "controller/ble_ll_utils.h" +#include "controller/ble_ll_conn.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_tmr.h" +#include "controller/ble_ll_hci.h" +#include "ble_ll_priv.h" +#include "ble_ll_cs_priv.h" + +extern struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap; +extern uint8_t g_ble_ll_cs_chan_count; +extern uint8_t g_ble_ll_cs_chan_indices[72]; +struct ble_ll_cs_sm *g_ble_ll_cs_sm_current; + +#define SUBEVENT_STATE_MODE0_STEP (0) +#define SUBEVENT_STATE_REPETITION_STEP (1) +#define SUBEVENT_STATE_SUBMODE_STEP (2) +#define SUBEVENT_STATE_MAINMODE_STEP (3) + +#define CS_SCHEDULE_NEW_STEP (0) +#define CS_SCHEDULE_NEW_SUBEVENT (1) +#define CS_SCHEDULE_NEW_EVENT (2) +#define CS_SCHEDULE_NEW_PROCEDURE (3) +#define CS_SCHEDULE_COMPLETED (4) + +#define SUBEVENT_DONE_STATUS_COMPLETED (0x0) +#define SUBEVENT_DONE_STATUS_PARTIAL (0x1) +#define SUBEVENT_DONE_STATUS_ABORTED (0xF) + +#define PROC_DONE_STATUS_COMPLETED (0x0) +#define PROC_DONE_STATUS_PARTIAL (0x1) +#define PROC_DONE_STATUS_ABORTED (0xF) + +#define PROC_ABORT_SUCCESS (0x00) +#define PROC_ABORT_REQUESTED (0x01) +#define PROC_ABORT_CHANNEL_MAP_CHANNELS (0x02) +#define PROC_ABORT_CHANNEL_MAP_UPDATE (0x03) +#define PROC_ABORT_UNSPECIFIED (0x0F) + +#define SUBEVENT_ABORT_SUCCESS (0x00) +#define SUBEVENT_ABORT_REQUESTED (0x01) +#define SUBEVENT_ABORT_NO_CS_SYNC (0x02) +#define SUBEVENT_ABORT_LIMITED_RESOURCES (0x03) +#define SUBEVENT_ABORT_UNSPECIFIED (0x0F) + +#define TIME_DIFF_NOT_AVAILABLE (0x00008000) + +#define BLE_LL_CONN_ITVL_USECS (1250) + +/* The ramp-down window in µs */ +#define T_RD (5) +/* The guard time duration in µs */ +#define T_GD (10) +/* The requency measurement period in µs */ +#define T_FM (80) + +/* Complement to full byte (4 bits) + header (16 bits) + CRC (24 bits) */ +#define BSIM_PACKET_OVERHEAD 4 + 16 + 24; + +static struct ble_ll_cs_aci aci_table[] = { + {1, 1, 1}, {2, 2, 1}, {3, 3, 1}, {4, 4, 1}, + {2, 1, 2}, {3, 1, 3}, {4, 1, 4}, {4, 2, 2} +}; +static const uint8_t rtt_seq_len[] = {0, 4, 12, 4, 8, 12, 16}; +static uint32_t tifs_table[6][6]; + +/* A pattern containing the states and transitions of the current step */ +static uint8_t rtt_buffer[300]; +static struct ble_ll_cs_step_transmission transmission_pattern[100]; +static struct ble_ll_cs_step subevent_steps[40]; + +/* For queueing the HCI Subevent Result (Continue) events */ +static struct ble_npl_event subevent_pool[MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING_SUBEVENT_EV_MAX_CNT)]; + +static int +ble_ll_cs_generate_channel(struct ble_ll_cs_sm *cssm, struct ble_ll_cs_step *step, + uint16_t steps_in_procedure_count) +{ + int rc; + uint8_t *channel_array; + uint8_t *next_channel_id; + uint8_t transaction_id; + + if (step->mode == BLE_LL_CS_MODE0) { + transaction_id = BLE_LL_CS_DRBG_HOP_CHAN_MODE0; + channel_array = cssm->mode0_channels; + next_channel_id = &cssm->mode0_next_chan_id; + } else { + transaction_id = BLE_LL_CS_DRBG_HOP_CHAN_NON_MODE0; + channel_array = cssm->non_mode0_channels; + next_channel_id = &cssm->non_mode0_next_chan_id; + } + + if (g_ble_ll_cs_chan_count <= *next_channel_id) { + rc = ble_ll_cs_drbg_shuffle_cr1(&cssm->drbg_ctx, steps_in_procedure_count, + transaction_id, g_ble_ll_cs_chan_indices, + channel_array, g_ble_ll_cs_chan_count); + if (rc) { + return rc; + } + + *next_channel_id = 0; + } + + step->channel = channel_array[(*next_channel_id)++]; + + return 0; +} + +static int +ble_ll_cs_backtracking_resistance(struct ble_ll_cs_sm *cssm) +{ + cssm->drbg_ctx.nonce_v[1] += BLE_LL_CS_DRBG_BACKTRACKING_RESISTANCE; + + return ble_ll_cs_drbg_f9(0, cssm->drbg_ctx.key, cssm->drbg_ctx.nonce_v); +} + +/** + * Called when scheduled event needs to be halted. This normally should not be called + * and is only called when a scheduled item executes but scanning for sync/chain + * is stil ongoing + * Context: Interrupt + */ +void +ble_ll_cs_proc_halt(void) +{ +} + +/** + * Called when a scheduled event has been removed from the scheduler + * without being run. + */ +void +ble_ll_cs_proc_rm_from_sched(void *cb_args) +{ +} + +static int +ble_ll_cs_init_subevent(struct ble_ll_cs_subevent *subevent, + struct ble_ll_cs_sm *cssm) +{ + struct ble_hci_ev_le_subev_cs_subevent_result *ev; + struct ble_hci_ev *hci_ev; + const struct ble_ll_cs_proc_params *params = &cssm->active_config->proc_params; + + if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT)) { + return 0; + } + + hci_ev = ble_transport_alloc_evt(0); + if (!hci_ev) { + return BLE_ERR_MEM_CAPACITY; + } + + 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_SUBEVENT_RESULT; + ev->conn_handle = htole16(cssm->connsm->conn_handle); + ev->config_id = cssm->active_config_id; + ev->start_acl_conn_event_counter = params->anchor_conn_event_cntr + + cssm->events_in_procedure_count * + params->event_interval; + ev->procedure_counter = cssm->procedure_count; + ev->frequency_compensation = 0xC000; + ev->reference_power_level = 0x7F; + ev->num_antenna_paths = cssm->n_ap; + ev->num_steps_reported = 0; + + memset(subevent, 0, sizeof(*subevent)); + subevent->hci_ev = hci_ev; + subevent->subev = BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT; + + return 0; +} + +static int +ble_ll_cs_init_subevent_continue(struct ble_ll_cs_subevent *subevent, + struct ble_ll_cs_sm *cssm) +{ + struct ble_hci_ev_le_subev_cs_subevent_result_continue *ev; + struct ble_hci_ev *hci_ev; + + if (!ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT_CONTINUE)) { + return 0; + } + + hci_ev = ble_transport_alloc_evt(0); + if (!hci_ev) { + return BLE_ERR_MEM_CAPACITY; + } + + 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_SUBEVENT_RESULT_CONTINUE; + ev->conn_handle = htole16(cssm->connsm->conn_handle); + ev->config_id = cssm->active_config_id; + ev->abort_reason = 0x00; + ev->num_antenna_paths = cssm->n_ap; + ev->num_steps_reported = 0; + + memset(subevent, 0, sizeof(*subevent)); + subevent->hci_ev = hci_ev; + subevent->subev = BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT_CONTINUE; + + return 0; +} + +/** + * Send HCI_LE_CS_Subevent_Result (or _Continue) + * + * Context: Link Layer task. + * + */ +static void +ble_ll_cs_proc_send_subevent(struct ble_npl_event *ev) +{ + struct ble_ll_cs_subevent *subevent; + struct ble_hci_ev *hci_ev; + + hci_ev = ble_npl_event_get_arg(ev); + BLE_LL_ASSERT(hci_ev); + + ble_ll_hci_event_send(hci_ev); + + ble_npl_event_set_arg(ev, NULL); +} + +static int +ble_ll_cs_proc_queue_subevent(struct ble_ll_cs_subevent *subevent, + uint8_t cs_schedule_status, + uint8_t proc_abort_reason, + uint8_t subev_abort_reason) +{ + uint8_t i; + struct ble_hci_ev *hci_ev = subevent->hci_ev; + struct ble_npl_event *ev; + uint8_t abort_reason = 0x00; + uint8_t procedure_done; + uint8_t subevent_done; + + switch (cs_schedule_status) { + case CS_SCHEDULE_NEW_STEP: + procedure_done = PROC_DONE_STATUS_PARTIAL; + subevent_done = SUBEVENT_DONE_STATUS_PARTIAL; + break; + case CS_SCHEDULE_NEW_SUBEVENT: + case CS_SCHEDULE_NEW_EVENT: + procedure_done = PROC_DONE_STATUS_PARTIAL; + subevent_done = SUBEVENT_DONE_STATUS_COMPLETED; + break; + case CS_SCHEDULE_NEW_PROCEDURE: + case CS_SCHEDULE_COMPLETED: + if (proc_abort_reason == PROC_ABORT_SUCCESS) { + procedure_done = PROC_DONE_STATUS_COMPLETED; + } else { + procedure_done = PROC_DONE_STATUS_ABORTED; + } + + if (subev_abort_reason == SUBEVENT_ABORT_SUCCESS) { + subevent_done = SUBEVENT_DONE_STATUS_COMPLETED; + } else { + subevent_done = SUBEVENT_DONE_STATUS_ABORTED; + } + + abort_reason = subev_abort_reason << 4 | proc_abort_reason; + break; + default: + BLE_LL_ASSERT(0); + } + + if (subevent->subev == BLE_HCI_LE_SUBEV_CS_SUBEVENT_RESULT) { + struct ble_hci_ev_le_subev_cs_subevent_result *ev = + (struct ble_hci_ev_le_subev_cs_subevent_result *)hci_ev->data; + + ev->procedure_done_status = procedure_done; + ev->subevent_done_status = subevent_done; + ev->abort_reason = abort_reason; + ev->num_steps_reported = subevent->num_steps_reported; + } else { + struct ble_hci_ev_le_subev_cs_subevent_result_continue *ev = + (struct ble_hci_ev_le_subev_cs_subevent_result_continue *)hci_ev->data; + + ev->procedure_done_status = procedure_done; + ev->subevent_done_status = subevent_done; + ev->abort_reason = abort_reason; + ev->num_steps_reported = subevent->num_steps_reported; + } + + memset(subevent, 0, sizeof(*subevent)); + + for (i = 0; i < ARRAY_SIZE(subevent_pool); ++i) { + ev = &subevent_pool[i]; + + if (ble_npl_event_get_arg(ev) == NULL) { + break; + } + + ev = NULL; + } + + if (!ev) { + ble_transport_free(hci_ev); + return 1; + } + + ble_npl_event_init(ev, ble_ll_cs_proc_send_subevent, hci_ev); + ble_ll_event_add(ev); + + return 0; +} + +static uint32_t +ble_ll_cs_proc_get_time_diff(struct ble_ll_cs_sm *cssm) +{ + struct ble_ll_cs_step* step = cssm->current_step; + struct ble_ll_cs_step_result *result = &cssm->step_result; + uint32_t arrival_us = result->time_of_arrival_us; + uint32_t departure_us = result->time_of_departure_us; + uint32_t arrival_ns = result->time_of_arrival_ns; + uint32_t departure_ns = result->time_of_departure_ns; + uint32_t time_diff_us; + uint32_t time_diff_ns; + uint32_t time_diff; + uint32_t nominal_offset_us; + bool invalid_time; + uint8_t role = cssm->active_config->role; + + if (step->mode == BLE_LL_CS_MODE1) { + nominal_offset_us = cssm->t_sy + cssm->t_sy_seq + T_RD + cssm->active_config->t_ip1; + } else if (step->mode == BLE_LL_CS_MODE3) { + /* TODO: Include CS tones into offset */ + nominal_offset_us = cssm->t_sy + cssm->t_sy_seq + T_RD + cssm->active_config->t_ip2; + } else { + return TIME_DIFF_NOT_AVAILABLE; + } + + invalid_time = (role == BLE_LL_CS_ROLE_INITIATOR) + ? (arrival_us < departure_us || (arrival_us == departure_us && arrival_ns <= departure_ns)) + : (arrival_us > departure_us || (arrival_us == departure_us && arrival_ns >= departure_ns)); + + if (invalid_time) { + return TIME_DIFF_NOT_AVAILABLE; + } + + time_diff_us = (role == BLE_LL_CS_ROLE_INITIATOR) + ? arrival_us - departure_us + : departure_us - arrival_us; + + /* The nominal offsets have to be removed (i.e., the interlude time + * between packets and the length of the packet itself) + */ + if (time_diff_us < nominal_offset_us) { + return TIME_DIFF_NOT_AVAILABLE; + } + + time_diff_ns = (time_diff_us - nominal_offset_us) * 1000 + + ((role == BLE_LL_CS_ROLE_INITIATOR) ? + arrival_ns - departure_ns : departure_ns - arrival_ns); + + /* Convert to units of 0.5ns */ + time_diff = time_diff_ns * 2; + return (time_diff > 0x7FFF) ? TIME_DIFF_NOT_AVAILABLE : time_diff; +} + +static void +ble_ll_cs_proc_add_step_result(struct ble_ll_cs_sm *cssm) +{ + int rc; + struct ble_hci_ev *hci_ev; + struct ble_ll_cs_step *step = cssm->current_step; + struct ble_ll_cs_config *conf = cssm->active_config; + struct ble_ll_cs_step_result *result = &cssm->step_result; + struct cs_steps_data *step_data; + uint8_t *data; + int32_t t_sy_center_delta_us = 0; + uint32_t time_diff; + uint8_t t_ip1 = conf->t_ip1; + uint8_t t_ip2 = conf->t_ip2; + uint8_t t_sw = cssm->t_sw; + uint8_t t_pm = conf->t_pm; + uint8_t n_ap = cssm->n_ap; + uint8_t t_sy = cssm->t_sy + cssm->t_sy_seq; + uint8_t data_len = 0; + uint8_t role = cssm->active_config->role; + uint8_t i; + + /* Estimate the size of the step results */ + if (step->mode == BLE_LL_CS_MODE0) { + data_len = 3; + if (role == BLE_LL_CS_ROLE_INITIATOR) { + data_len += 2; + } + } else if (step->mode == BLE_LL_CS_MODE1) { + data_len = 6; + if (result->sounding_pct_estimate) { + data_len += 8; + } + t_sy_center_delta_us = t_sy + T_RD + t_ip1; + } else if (step->mode == BLE_LL_CS_MODE2) { + data_len = 1 + (n_ap + 1) * 3 * 2; + } else if (step->mode == BLE_LL_CS_MODE3) { + data_len = 6 + 1 + (n_ap + 1) * 3 * 2; + if (result->sounding_pct_estimate) { + data_len += 8; + } + t_sy_center_delta_us = t_sy + T_RD + 2 * (t_sw + t_pm) * (n_ap + 1) + t_ip2; + } + + BLE_LL_ASSERT(cssm->buffered_subevent.hci_ev); + hci_ev = cssm->buffered_subevent.hci_ev; + + /* Validate if the step results will fit into the current hci event + * buffer or mark it as pending and create a new buffer. + */ + if (hci_ev->length + data_len <= BLE_HCI_MAX_DATA_LEN) { + ++cssm->buffered_subevent.num_steps_reported; + } else { + BLE_LL_ASSERT(cssm->steps_in_subevent_count != 0); + + cssm->cs_schedule_status = CS_SCHEDULE_NEW_STEP; + rc = ble_ll_cs_proc_queue_subevent(&cssm->buffered_subevent, cssm->cs_schedule_status, + PROC_ABORT_SUCCESS, SUBEVENT_ABORT_SUCCESS); + BLE_LL_ASSERT(rc == 0); + + ble_ll_cs_init_subevent_continue(&cssm->buffered_subevent, cssm); + hci_ev = cssm->buffered_subevent.hci_ev; + + BLE_LL_ASSERT(hci_ev); + BLE_LL_ASSERT(hci_ev->length + data_len <= BLE_HCI_MAX_DATA_LEN); + + cssm->buffered_subevent.hci_ev = hci_ev; + cssm->buffered_subevent.num_steps_reported = 1; + } + + step_data = (struct cs_steps_data *)(hci_ev->data + hci_ev->length); + step_data->mode = step->mode; + step_data->channel = step->channel; + data = step_data->data; + + /* Get ToA_ToD_Initiator/ToD_ToA_Reflector */ + time_diff = ble_ll_cs_proc_get_time_diff(cssm); + /* TODO: Use this */ + (void)t_sy_center_delta_us; + + /* Pack the step results into the buffered hci event */ + if (step->mode == BLE_LL_CS_MODE0) { + data[0] = result->packet_quality; + data[1] = result->packet_rssi; + data[2] = cssm->cs_sync_antenna; + data_len = 3; + + if (role == BLE_LL_CS_ROLE_INITIATOR) { + put_le16(data + 3, result->measured_freq_offset); + data_len += 2; + } + + } else if (step->mode == BLE_LL_CS_MODE1) { + data[0] = result->packet_quality; + data[1] = result->packet_nadm; + data[2] = result->packet_rssi; + put_le16(data + 3, time_diff); + data[5] = cssm->cs_sync_antenna; + data_len = 6; + + if (result->sounding_pct_estimate) { + put_le32(data + 6, result->packet_pct1); + put_le32(data + 10, result->packet_pct2); + data_len += 8; + } + + } else if (step->mode == BLE_LL_CS_MODE2) { + data[0] = cssm->active_config->proc_params.aci; + data_len = 1; + + for (i = 0; i < cssm->n_ap + 1; ++i) { + put_le24(data + data_len, result->tone_pct[i]); + data_len += 3; + } + + for (i = 0; i < cssm->n_ap + 1; ++i) { + data[data_len] = result->tone_quality_ind[i]; + ++data_len; + } + + } else if (step->mode == BLE_LL_CS_MODE3) { + data[0] = result->packet_quality; + data[1] = result->packet_nadm; + data[2] = result->packet_rssi; + put_le16(data + 3, time_diff); + data[5] = cssm->cs_sync_antenna; + data_len = 6; + + if (result->sounding_pct_estimate) { + put_le32(data + 6, result->packet_pct1); + put_le32(data + 10, result->packet_pct2); + data_len += 8; + } + + data[data_len] = cssm->active_config->proc_params.aci; + data_len += 1; + + for (i = 0; i < cssm->n_ap + 1; ++i) { + put_le24(data + data_len, result->tone_pct[i]); + data_len += 3; + } + + for (i = 0; i < cssm->n_ap + 1; ++i) { + data[data_len++] = result->tone_quality_ind[i]; + } + } + + hci_ev->length += data_len; + step_data->data_len = data_len; + memset(result, 0, sizeof(*result)); +} + +static uint8_t +ble_ll_cs_proc_set_t_sw(struct ble_ll_cs_sm *cssm) +{ + uint8_t t_sw; + uint8_t t_sw_i; + uint8_t t_sw_r; + uint8_t aci = cssm->active_config->proc_params.aci; + + if (cssm->active_config->role == BLE_LL_CS_ROLE_INITIATOR) { + t_sw_i = g_ble_ll_cs_local_cap.t_sw; + t_sw_r = cssm->remote_cap.t_sw; + } else { /* BLE_LL_CS_ROLE_REFLECTOR */ + t_sw_i = cssm->remote_cap.t_sw; + t_sw_r = g_ble_ll_cs_local_cap.t_sw; + } + + if (aci == 0) { + t_sw = 0; + } else if (IN_RANGE(aci, 1, 3)) { + t_sw = t_sw_i; + } else if (IN_RANGE(aci, 4, 6)) { + t_sw = t_sw_r; + } else { /* ACI == 7 */ + if (g_ble_ll_cs_local_cap.t_sw > cssm->remote_cap.t_sw) { + t_sw = g_ble_ll_cs_local_cap.t_sw; + } else { + t_sw = cssm->remote_cap.t_sw; + } + } + + cssm->t_sw = t_sw; + + return t_sw; +} + +static void +ble_ll_cs_proc_tifs_init(uint32_t t_ip1, uint32_t t_ip2, uint32_t t_sw, uint32_t t_fcs) +{ + memset(tifs_table, 0, sizeof(tifs_table)); + + tifs_table[STEP_STATE_CS_SYNC_I][STEP_STATE_CS_SYNC_R] = T_RD + t_ip1; + tifs_table[STEP_STATE_CS_SYNC_I][STEP_STATE_CS_TONE_I] = T_GD; + + tifs_table[STEP_STATE_CS_SYNC_R][STEP_STATE_CS_TONE_R] = T_GD; + tifs_table[STEP_STATE_CS_SYNC_R][STEP_STATE_COMPLETE] = T_RD + t_fcs; + + tifs_table[STEP_STATE_CS_TONE_I][STEP_STATE_CS_TONE_I] = t_sw; + tifs_table[STEP_STATE_CS_TONE_I][STEP_STATE_CS_TONE_R] = T_RD + t_ip2; + + tifs_table[STEP_STATE_CS_TONE_R][STEP_STATE_CS_SYNC_R] = T_GD; + tifs_table[STEP_STATE_CS_TONE_R][STEP_STATE_CS_TONE_R] = t_sw; + tifs_table[STEP_STATE_CS_TONE_R][STEP_STATE_COMPLETE] = T_RD + t_fcs; +} + +static uint32_t +ble_ll_cs_proc_tifs_get(uint8_t old_state, uint8_t new_state) +{ + BLE_LL_ASSERT(old_state < 6 && new_state < 6); + return tifs_table[old_state][new_state]; +} + +static int +ble_ll_cs_proc_calculate_timing(struct ble_ll_cs_sm *cssm) +{ + struct ble_ll_cs_config *conf = cssm->active_config; + const struct ble_ll_cs_proc_params *params = &conf->proc_params; + uint8_t t_fcs = conf->t_fcs; + uint8_t t_ip1 = conf->t_ip1; + uint8_t t_ip2 = conf->t_ip2; + uint8_t t_pm = conf->t_pm; + uint8_t t_sw; + uint8_t n_ap = cssm->n_ap; + uint8_t t_sy; + uint8_t t_sy_seq; + uint8_t sequence_len; + + t_sw = ble_ll_cs_proc_set_t_sw(cssm); + + /* CS packets with no Sounding Sequence or Random Sequence fields take 44 µs + * to transmit when sent using the LE 1M PHY and 26 µs to transmit when using + * the LE 2M and the LE 2M 2BT PHYs. CS packets that include a Sounding Sequence + * or Random Sequence field take proportionally longer to transmit based on + * the length of the field and the PHY selection. + */ + + sequence_len = rtt_seq_len[conf->rtt_type]; + + switch (conf->cs_sync_phy) { + case BLE_LL_CS_SYNC_PHY_1M: + t_sy = BLE_LL_CS_SYNC_TIME_1M; + t_sy_seq = sequence_len; + break; + case BLE_LL_CS_SYNC_PHY_2M: + t_sy = BLE_LL_CS_SYNC_TIME_2M; + t_sy_seq = sequence_len / 2; + break; + default: + BLE_LL_ASSERT(0); + } + + cssm->mode_duration_usecs[BLE_LL_CS_MODE0] = + t_ip1 + T_GD + T_FM + 2 * (t_sy + T_RD) + t_fcs; + + cssm->mode_duration_usecs[BLE_LL_CS_MODE1] = + t_ip1 + 2 * (t_sy + t_sy_seq + T_RD) + t_fcs; + + cssm->mode_duration_usecs[BLE_LL_CS_MODE2] = + t_ip2 + 2 * ((t_sw + t_pm) * (n_ap + 1) + T_RD) + t_fcs; + + cssm->mode_duration_usecs[BLE_LL_CS_MODE3] = + t_ip2 + 2 * ((t_sy + t_sy_seq + T_GD + T_RD) + + (t_sw + t_pm) * (n_ap + 1)) + t_fcs; + + cssm->t_sy = t_sy; + cssm->t_sy_seq = t_sy_seq; + + cssm->subevent_interval_usecs = params->subevent_interval * BLE_LL_CS_SUBEVENTS_INTERVAL_UNIT_US; + + cssm->event_interval_usecs = params->event_interval * cssm->connsm->conn_itvl * + BLE_LL_CONN_ITVL_USECS; + + cssm->procedure_interval_usecs = params->procedure_interval * cssm->connsm->conn_itvl * + BLE_LL_CONN_ITVL_USECS; + +#if BABBLESIM + cssm->mode_duration_usecs[BLE_LL_CS_MODE0] += BSIM_PACKET_OVERHEAD; + cssm->mode_duration_usecs[BLE_LL_CS_MODE1] += BSIM_PACKET_OVERHEAD; + cssm->mode_duration_usecs[BLE_LL_CS_MODE2] += BSIM_PACKET_OVERHEAD; + cssm->mode_duration_usecs[BLE_LL_CS_MODE3] += BSIM_PACKET_OVERHEAD; +#endif + + return 0; +} + +static uint32_t +ble_ll_cs_proc_step_state_duration_get(uint8_t state, uint8_t mode, + uint8_t t_sy, uint8_t t_sy_seq) +{ + uint32_t duration = 0; + +#if BABBLESIM + t_sy += BSIM_PACKET_OVERHEAD; +#endif + + switch (state) { + case STEP_STATE_CS_SYNC_I: + case STEP_STATE_CS_SYNC_R: + duration = t_sy; + if (mode != BLE_LL_CS_MODE0) { + duration += t_sy_seq; + } + break; + case STEP_STATE_CS_TONE_I: + case STEP_STATE_CS_TONE_R: + duration = T_FM; + break; + case STEP_STATE_INIT: + case STEP_STATE_COMPLETE: + duration = 0; + break; + default: + BLE_LL_ASSERT(0); + } + + return duration; +} + +static uint8_t +ble_ll_cs_proc_mode0_next_state(uint8_t state) +{ + switch (state) { + case STEP_STATE_INIT: + state = STEP_STATE_CS_SYNC_I; + break; + case STEP_STATE_CS_SYNC_I: + state = STEP_STATE_CS_SYNC_R; + break; + case STEP_STATE_CS_SYNC_R: + state = STEP_STATE_CS_TONE_R; + break; + case STEP_STATE_CS_TONE_R: + state = STEP_STATE_COMPLETE; + break; + default: + BLE_LL_ASSERT(0); + } + + return state; +} + +static uint8_t +ble_ll_cs_proc_mode1_next_state(uint8_t state) +{ + switch (state) { + case STEP_STATE_INIT: + state = STEP_STATE_CS_SYNC_I; + break; + case STEP_STATE_CS_SYNC_I: + state = STEP_STATE_CS_SYNC_R; + break; + case STEP_STATE_CS_SYNC_R: + state = STEP_STATE_COMPLETE; + break; + default: + BLE_LL_ASSERT(0); + } + + return state; +} + +static uint8_t +ble_ll_cs_proc_mode2_next_state(uint8_t state, uint8_t slot_count) +{ + switch (state) { + case STEP_STATE_INIT: + state = STEP_STATE_CS_TONE_I; + break; + case STEP_STATE_CS_TONE_I: + if (slot_count == 0) { + state = STEP_STATE_CS_TONE_R; + } + break; + case STEP_STATE_CS_TONE_R: + if (slot_count == 0) { + state = STEP_STATE_COMPLETE; + } + break; + default: + BLE_LL_ASSERT(0); + } + + return state; +} + +static uint8_t +ble_ll_cs_proc_mode3_next_state(uint8_t state, uint8_t slot_count) +{ + switch (state) { + case STEP_STATE_INIT: + state = STEP_STATE_CS_SYNC_I; + break; + case STEP_STATE_CS_SYNC_I: + state = STEP_STATE_CS_TONE_I; + break; + case STEP_STATE_CS_TONE_I: + if (slot_count == 0) { + state = STEP_STATE_CS_TONE_R; + } + break; + case STEP_STATE_CS_TONE_R: + if (slot_count == 0) { + state = STEP_STATE_CS_SYNC_R; + } + break; + case STEP_STATE_CS_SYNC_R: + state = STEP_STATE_COMPLETE; + break; + default: + BLE_LL_ASSERT(0); + } + + return state; +} + +static uint8_t +ble_ll_cs_proc_next_step_state_get(uint8_t mode, uint8_t state, uint8_t slot_count) +{ + switch (mode) { + case BLE_LL_CS_MODE0: + state = ble_ll_cs_proc_mode0_next_state(state); + break; + case BLE_LL_CS_MODE1: + state = ble_ll_cs_proc_mode1_next_state(state); + break; +#if CS_TONE_SUPPORT + case BLE_LL_CS_MODE2: + state = ble_ll_cs_proc_mode2_next_state(state, slot_count); + break; + case BLE_LL_CS_MODE3: + state = ble_ll_cs_proc_mode3_next_state(state, slot_count); + break; +#endif + default: + BLE_LL_ASSERT(0); + } + + return state; +} + +static void +ble_ll_cs_transm_mode_set(uint8_t role, uint8_t step_state, + struct ble_ll_cs_step *step, + struct ble_ll_cs_step_transmission *step_transm) +{ + struct ble_phy_cs_transmission *transm = &step_transm->phy_transm; + bool is_initiator = (role == BLE_LL_CS_ROLE_INITIATOR); + + transm->channel = step->channel; + + switch (step_state) { + case STEP_STATE_CS_SYNC_I: + transm->mode = BLE_PHY_CS_TRANSM_MODE_SYNC; + transm->is_tx = is_initiator; + transm->aa = step->initiator_aa; + break; + case STEP_STATE_CS_SYNC_R: + transm->mode = BLE_PHY_CS_TRANSM_MODE_SYNC; + transm->is_tx = !is_initiator; + transm->aa = step->reflector_aa; + break; + case STEP_STATE_CS_TONE_I: + transm->mode = BLE_PHY_CS_TRANSM_MODE_TONE; + transm->is_tx = is_initiator; + break; + case STEP_STATE_CS_TONE_R: + transm->mode = BLE_PHY_CS_TRANSM_MODE_TONE; + transm->is_tx = !is_initiator; + + transm->tone_mode = BLE_PHY_CS_TONE_MODE_FM; + if (step->mode == BLE_LL_CS_MODE0) { + transm->tone_mode = BLE_PHY_CS_TONE_MODE_FM; + } else { + transm->tone_mode = BLE_PHY_CS_TONE_MODE_PM; + } + break; + default: + BLE_LL_ASSERT(0); + } +} + +int ble_ll_cs_rtt_generate(struct ble_ll_cs_drbg_ctx *drbg_ctx, struct ble_ll_cs_step *step, + uint8_t *buf, uint8_t *out_rtt_len, uint32_t buf_len, + uint16_t steps_in_procedure_count, uint8_t rtt_type, uint8_t role); + +static int +ble_ll_cs_proc_step_transmission_generate(struct ble_ll_cs_step *prev_step, struct ble_ll_cs_step *step, + struct ble_ll_cs_step_transmission *last_transm_slot, + uint8_t role, uint8_t n_ap, uint8_t t_sy, uint8_t t_sy_seq) +{ + struct ble_ll_cs_step_transmission *prev_transm; + struct ble_ll_cs_step_transmission *transm; + uint32_t duration_usecs; + uint32_t end_tifs; + uint8_t prev_state; + uint8_t next_state; + uint8_t slot_count; + uint8_t skip_transm; + + prev_transm = (prev_step == NULL) ? step->next_transm : prev_step->last_transm; + transm = step->next_transm; + slot_count = n_ap; + + prev_state = STEP_STATE_INIT; + next_state = ble_ll_cs_proc_next_step_state_get(step->mode, prev_state, slot_count); + + while (next_state != STEP_STATE_COMPLETE) { + BLE_LL_ASSERT(transm != last_transm_slot); + skip_transm = false; + + if (next_state == STEP_STATE_CS_TONE_I || next_state == STEP_STATE_CS_TONE_R) { + if (slot_count == 0) { + slot_count = n_ap; + + if ((next_state == STEP_STATE_CS_TONE_I) ? step->tone_ext_presence_i + : step->tone_ext_presence_r) { + skip_transm = true; + } + } else { + --slot_count; + } + } + + duration_usecs = ble_ll_cs_proc_step_state_duration_get(next_state, step->mode, + t_sy, t_sy_seq); + end_tifs = ble_ll_cs_proc_tifs_get(prev_state, next_state); + +#if BABBLESIM || !CS_TONE_SUPPORT + if (next_state == STEP_STATE_CS_TONE_I || next_state == STEP_STATE_CS_TONE_R) { + skip_transm = true; + } +#endif + + if (skip_transm) { + /* If a transmission slot should be skipped, just add up the T_IFS and its duration */ + prev_transm->phy_transm.end_tifs += duration_usecs + end_tifs; + } else { + /* Set the end transition for the previous transmission */ + prev_transm->phy_transm.end_tifs += end_tifs; + prev_transm->phy_transm.next = &transm->phy_transm; + + /* Next transmission */ + transm->state = next_state; + transm->phy_transm.duration_usecs = duration_usecs; + transm->phy_transm.end_tifs = 0; + ble_ll_cs_transm_mode_set(role, next_state, step, transm); + + prev_transm = transm++; + } + + prev_state = next_state; + next_state = ble_ll_cs_proc_next_step_state_get(step->mode, prev_state, slot_count); + } + + BLE_LL_ASSERT(next_state == STEP_STATE_COMPLETE); + + prev_transm->phy_transm.end_tifs += ble_ll_cs_proc_tifs_get(prev_state, next_state); + step->last_transm = prev_transm; + transm->state = STEP_STATE_COMPLETE; + + return 0; +} + +static int +ble_ll_cs_step_generate(struct ble_ll_cs_sm *cssm, struct ble_ll_cs_step *step, + uint8_t subevent_state, uint16_t steps_in_procedure_count) +{ + int rc; + struct ble_ll_cs_step_transmission *last_transm_slot; + const struct ble_ll_cs_config *conf = cssm->active_config; + uint8_t *rtt_buf_end = rtt_buffer + sizeof(rtt_buffer) / sizeof(rtt_buffer[0]); + + if (conf->sub_mode != 0xFF && subevent_state == SUBEVENT_STATE_MAINMODE_STEP) { + if (cssm->main_step_count == 0xFF) { + /* Rand the number of Main_Mode steps to execute + * before a Sub_Mode insertion. + */ + rc = ble_ll_cs_drbg_rand_main_mode_steps( + &cssm->drbg_ctx, steps_in_procedure_count, + conf->main_mode_min_steps, conf->main_mode_max_steps, + &cssm->main_step_count); + + if (rc) { + return rc; + } + } + + --cssm->main_step_count; + } + + if (subevent_state != SUBEVENT_STATE_REPETITION_STEP) { + rc = ble_ll_cs_generate_channel(cssm, step, steps_in_procedure_count); + if (rc) { + return rc; + } + + /* Update channel cache used in repetition steps */ + if (subevent_state == SUBEVENT_STATE_MAINMODE_STEP && + conf->main_mode_repetition) { + cssm->repetition_channels[0] = cssm->repetition_channels[1]; + cssm->repetition_channels[1] = cssm->repetition_channels[2]; + cssm->repetition_channels[2] = step->channel; + } + } else { + step->channel = cssm->repetition_channels[ + conf->main_mode_repetition - cssm->repetition_count]; + } + + if (step->mode == BLE_LL_CS_MODE0 || step->mode == BLE_LL_CS_MODE1 || + step->mode == BLE_LL_CS_MODE3) { + /* Generate CS Access Address */ + rc = ble_ll_cs_drbg_generate_aa(&cssm->drbg_ctx, + steps_in_procedure_count, + &step->initiator_aa, &step->reflector_aa); + if (rc) { + return rc; + } + + /* TODO: Generate antenna ID if multiple antennas available */ + cssm->cs_sync_antenna = 0x01; + } + + if (step->mode == BLE_LL_CS_MODE2 || step->mode == BLE_LL_CS_MODE3) { + rc = ble_ll_cs_drbg_rand_tone_ext_presence( + &cssm->drbg_ctx, steps_in_procedure_count, &step->tone_ext_presence_i); + if (rc) { + return rc; + } + + rc = ble_ll_cs_drbg_rand_tone_ext_presence( + &cssm->drbg_ctx, steps_in_procedure_count, &step->tone_ext_presence_r); + if (rc) { + return rc; + } + } + + if (step->mode == BLE_LL_CS_MODE1 || step->mode == BLE_LL_CS_MODE3) { + step->rtt_tx = cssm->next_rtt_ptr; + ble_ll_cs_rtt_generate(&cssm->drbg_ctx, step, step->rtt_tx, &step->rtt_tx_len, + rtt_buf_end - cssm->next_rtt_ptr, steps_in_procedure_count, + conf->rtt_type, conf->role); + cssm->next_rtt_ptr += step->rtt_tx_len; + + step->rtt_rx = cssm->next_rtt_ptr; + ble_ll_cs_rtt_generate(&cssm->drbg_ctx, step, step->rtt_rx, &step->rtt_rx_len, + rtt_buf_end - cssm->next_rtt_ptr, steps_in_procedure_count, + conf->rtt_type, conf->role); + cssm->next_rtt_ptr += step->rtt_rx_len; + } + + last_transm_slot = transmission_pattern + + sizeof(transmission_pattern) / sizeof(transmission_pattern[0]) - 1; + + ble_ll_cs_proc_step_transmission_generate(cssm->last_step, step, last_transm_slot, conf->role, + cssm->n_ap, cssm->t_sy, cssm->t_sy_seq); + + return 0; +} + +static int +ble_ll_cs_setup_next_subevent(struct ble_ll_cs_sm *cssm) +{ + cssm->steps_in_subevent_count = 0; + + cssm->subevent_anchor_usecs += cssm->subevent_interval_usecs; + cssm->step_anchor_usecs = cssm->subevent_anchor_usecs; + cssm->anchor_usecs = cssm->step_anchor_usecs; + + return 0; +} + +static int +ble_ll_cs_setup_next_event(struct ble_ll_cs_sm *cssm) +{ + cssm->cs_schedule_status = CS_SCHEDULE_NEW_EVENT; + ++cssm->events_in_procedure_count; + cssm->subevents_in_event_count = 0; + cssm->steps_in_subevent_count = 0; + + cssm->event_anchor_usecs += cssm->event_interval_usecs; + cssm->subevent_anchor_usecs = cssm->event_anchor_usecs; + cssm->step_anchor_usecs = cssm->event_anchor_usecs; + cssm->anchor_usecs = cssm->step_anchor_usecs; + + return 0; +} + +static int +ble_ll_cs_setup_next_procedure(struct ble_ll_cs_sm *cssm) +{ + const struct ble_ll_cs_config *conf = cssm->active_config; + + if (cssm->procedure_count + 1 >= conf->proc_params.max_procedure_count || + cssm->terminate_measurement) { + /* All CS procedures have been completed or + * the CS procedure repeat series has been terminated. + */ + cssm->cs_schedule_status = CS_SCHEDULE_COMPLETED; + + if (cssm->terminate_measurement) { + cssm->proc_abort_reason = PROC_ABORT_REQUESTED; + } + + return 1; + } + + cssm->cs_schedule_status = CS_SCHEDULE_NEW_PROCEDURE; + ++cssm->procedure_count; + cssm->events_in_procedure_count = 0; + cssm->subevents_in_procedure_count = 0; + cssm->subevents_in_event_count = 0; + cssm->steps_in_procedure_count = 0; + cssm->steps_in_subevent_count = 0; + cssm->main_step_count = 0xFF; + + cssm->procedure_anchor_usecs += cssm->procedure_interval_usecs; + cssm->step_anchor_usecs = cssm->procedure_anchor_usecs; + cssm->subevent_anchor_usecs = cssm->step_anchor_usecs; + cssm->anchor_usecs = cssm->step_anchor_usecs; + + return ble_ll_cs_backtracking_resistance(cssm); +} + +static int +ble_ll_cs_proc_subevent_generate(struct ble_ll_cs_sm *cssm) +{ + int rc; + struct ble_ll_cs_step *step; + const struct ble_ll_cs_config *conf = cssm->active_config; + const struct ble_ll_cs_proc_params *params = &conf->proc_params; + + uint32_t subevent_duration = 0; + uint32_t new_subevent_duration = 0; + uint32_t procedure_duration; + uint32_t max_subevent_duration = params->subevent_len; + uint32_t max_procedure_duration = params->max_procedure_len * BLE_LL_CS_PROCEDURE_LEN_UNIT_US; + + uint8_t step_mode; + uint8_t subevent_state; + uint8_t steps_in_subevent = 0; + uint8_t mode0_step_count = conf->mode_0_steps; + + cssm->cs_schedule_status = CS_SCHEDULE_NEW_SUBEVENT; + ++cssm->subevents_in_procedure_count; + ++cssm->subevents_in_event_count; + + if (cssm->current_step == NULL) { + /* Setup the first subevent */ + cssm->steps_in_procedure_count = 0; + cssm->steps_in_subevent_count = 0; + cssm->subevents_in_procedure_count = 0; + cssm->subevents_in_event_count = 0; + cssm->procedure_count = 0; + } else if (cssm->subevents_in_procedure_count + 1 > BLE_LL_CS_SUBEVENTS_PER_PROCEDURE_MAX) { + /* No more subevents will fit inside the current CS procedure */ + rc = ble_ll_cs_setup_next_procedure(cssm); + if (rc) { + /* No more procedures can be generated */ + return rc; + } + } else if (cssm->subevents_in_event_count + 1 > params->subevents_per_event) { + /* No more subevents will fit inside the current CS event */ + ble_ll_cs_setup_next_event(cssm); + } else { + /* Setup a new subevent */ + ble_ll_cs_setup_next_subevent(cssm); + } + + procedure_duration = cssm->anchor_usecs - cssm->procedure_anchor_usecs; + + if (cssm->subevents_in_procedure_count == 0) { + cssm->main_step_count = 0xFF; + } else { + cssm->repetition_count = conf->main_mode_repetition; + } + + ble_ll_cs_drbg_clear_cache(&cssm->drbg_ctx); + memset(transmission_pattern, 0, sizeof(transmission_pattern)); + memset(subevent_steps, 0, sizeof(subevent_steps)); + step = &subevent_steps[0]; + step->next_transm = &transmission_pattern[0]; + cssm->current_step = step; + cssm->last_step = NULL; + cssm->next_rtt_ptr = rtt_buffer; + + while (true) { + ++steps_in_subevent; + BLE_LL_ASSERT(steps_in_subevent <= sizeof(subevent_steps)); + + /* Determine next step mode */ + if (mode0_step_count > 0) { + subevent_state = SUBEVENT_STATE_MODE0_STEP; + step_mode = BLE_LL_CS_MODE0; + --mode0_step_count; + + } else if (cssm->repetition_count > 0) { + subevent_state = SUBEVENT_STATE_REPETITION_STEP; + step_mode = conf->main_mode; + --cssm->repetition_count; + + } else if (cssm->main_step_count == 0) { + subevent_state = SUBEVENT_STATE_SUBMODE_STEP; + step_mode = conf->sub_mode; + cssm->main_step_count = 0xFF; + + } else { + subevent_state = SUBEVENT_STATE_MAINMODE_STEP; + step_mode = conf->main_mode; + } + + new_subevent_duration = subevent_duration + cssm->mode_duration_usecs[step_mode]; + + /* Check if this step will fit inside the current subevent/procedure */ + if (cssm->steps_in_procedure_count + steps_in_subevent > BLE_LL_CS_STEPS_PER_PROCEDURE_MAX || + procedure_duration + new_subevent_duration > max_procedure_duration || + steps_in_subevent > BLE_LL_CS_STEPS_PER_SUBEVENT_MAX || + new_subevent_duration > max_subevent_duration) { + + /* TODO: Implement the remaining conditions to complete the procedure: + * • If Channel Selection Algorithm #3b is used for non-mode-0 steps, and the channel + * map generated from CSFilteredChM has been used for CSNumRepetitions cycles + * for non-mode‑0 steps including the use for both Main_Mode and Sub_Mode steps. + * • If Channel Selection Algorithm #3c is used for non-mode-0 steps, and + * CSNumRepetitions invocations of Channel Selection Algorithm #3c have been + * completed for non-mode‑0 steps including the use for both Main_Mode and + * Sub_Mode steps. + */ + + if (steps_in_subevent > 1) { + /* Compelete generation of the current subevent. It will be the last one + * in the current procedure. + */ + --steps_in_subevent; + + break; + } + + /* Skip this subevent and generate a new one in a new procedure */ + rc = ble_ll_cs_setup_next_procedure(cssm); + if (rc) { + /* No more procedures can be generated */ + return rc; + } + + steps_in_subevent = 0; + mode0_step_count = conf->mode_0_steps; + cssm->repetition_count = conf->main_mode_repetition; + procedure_duration = cssm->anchor_usecs - cssm->procedure_anchor_usecs; + + continue; + } + + subevent_duration = new_subevent_duration; + step->mode = step_mode; + + ble_ll_cs_step_generate(cssm, step, subevent_state, + cssm->steps_in_procedure_count + steps_in_subevent - 1); + + cssm->last_step = step++; + step->next_transm = cssm->last_step->last_transm + 1; + } + + return 0; +} + +static int +ble_ll_cs_proc_next_state(struct ble_ll_cs_sm *cssm) +{ + int rc; + struct ble_ll_cs_step *step = cssm->current_step; + + if (step != NULL) { + if (step->next_transm != step->last_transm) { + /* Continue with the next transmission of the current step */ + ++step->next_transm; + return 0; + } + + /* Save the step results */ + ble_ll_cs_proc_add_step_result(cssm); + ++cssm->steps_in_procedure_count; + ++cssm->steps_in_subevent_count; + + if (cssm->current_step != cssm->last_step) { + /* Continue with the next step */ + ++cssm->current_step; + return 0; + } + } + + ble_ll_state_set(BLE_LL_STATE_STANDBY); + + /* Generate new subevent steps */ + rc = ble_ll_cs_proc_subevent_generate(cssm); + if (rc) { + if (!cssm->proc_abort_reason && cssm->cs_schedule_status != CS_SCHEDULE_COMPLETED) { + cssm->proc_abort_reason = PROC_ABORT_UNSPECIFIED; + } + + cssm->cs_schedule_status = CS_SCHEDULE_COMPLETED; + rc = ble_ll_cs_proc_queue_subevent(&cssm->buffered_subevent, cssm->cs_schedule_status, + cssm->proc_abort_reason, cssm->subev_abort_reason); + BLE_LL_ASSERT(rc == 0); + + return 1; + } + + if (step != NULL) { + rc = ble_ll_cs_proc_queue_subevent(&cssm->buffered_subevent, cssm->cs_schedule_status, + cssm->proc_abort_reason, cssm->subev_abort_reason); + BLE_LL_ASSERT(rc == 0); + } + + rc = ble_ll_cs_init_subevent(&cssm->buffered_subevent, cssm); + BLE_LL_ASSERT(rc == 0); + + return 0; +} + +int +ble_ll_cs_proc_subevent_schedule(struct ble_ll_cs_sm *cssm) +{ + int rc; + uint32_t cputime; + struct ble_ll_cs_step *step = cssm->current_step; + struct ble_ll_cs_step_transmission *transm = step->next_transm; + uint8_t rem_us; + + cputime = ble_ll_tmr_u2t_r(cssm->anchor_usecs, &rem_us); + + ble_ll_tx_power_set(g_ble_ll_tx_power); + + rc = ble_phy_cs_subevent_start(&transm->phy_transm, cputime, rem_us); + if (rc) { + ble_ll_cs_proc_sync_lost(cssm); + return 1; + } + + ble_ll_state_set(BLE_LL_STATE_CS); + + return 0; +} + +static int +ble_ll_cs_proc_sched_cb(struct ble_ll_sched_item *sch) +{ + int rc; + struct ble_ll_cs_sm *cssm = sch->cb_arg; + + BLE_LL_ASSERT(cssm != NULL); + + rc = cssm->sched_cb(cssm); + if (rc) { + return BLE_LL_SCHED_STATE_DONE; + } + + return BLE_LL_SCHED_STATE_RUNNING; +} + +int +ble_ll_cs_proc_schedule_next_tx_or_rx(struct ble_ll_cs_sm *cssm) +{ + int rc; + struct ble_phy_cs_transmission *transm; + uint32_t anchor_cputime; + uint8_t transition; + uint8_t offset; + + rc = ble_ll_cs_proc_next_state(cssm); + if (rc || cssm->cs_schedule_status == CS_SCHEDULE_COMPLETED) { + ble_phy_disable(); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + + return rc; + } + + if (cssm->cs_schedule_status == CS_SCHEDULE_NEW_STEP) { + return 0; + } + cssm->cs_schedule_status = CS_SCHEDULE_NEW_STEP; + + transm = &cssm->current_step->next_transm->phy_transm; + anchor_cputime = ble_ll_tmr_u2t(cssm->anchor_usecs); + + if (anchor_cputime - g_ble_ll_sched_offset_ticks < ble_ll_tmr_get()) { + ble_phy_disable(); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + return 1; + } + + if (!transm->is_tx) { + /* Every CS subevent begins with mode 0 step. Start RX window earlier */ + offset = 2; + cssm->anchor_usecs -= offset; + transm->duration_usecs += offset; + } + + cssm->sched_cb = ble_ll_cs_proc_subevent_schedule; + cssm->sch.start_time = anchor_cputime - g_ble_ll_sched_offset_ticks; + cssm->sch.end_time = anchor_cputime + ble_ll_tmr_u2t_up(transm->duration_usecs); + cssm->sch.remainder = 0; + cssm->sch.sched_type = BLE_LL_SCHED_TYPE_CS; + cssm->sch.cb_arg = cssm; + cssm->sch.sched_cb = ble_ll_cs_proc_sched_cb; + rc = ble_ll_sched_cs_proc(&cssm->sch); + + return rc; +} + +void +ble_ll_cs_proc_set_now_as_anchor_point(struct ble_ll_cs_sm *cssm) +{ + cssm->anchor_usecs = ble_ll_tmr_t2u(ble_ll_tmr_get()); +} + +static int +ble_ll_cs_proc_schedule_first_subevent(struct ble_ll_cs_sm *cssm) +{ + int rc; + struct ble_ll_conn_sm *connsm = cssm->connsm; + struct ble_ll_cs_config *conf = cssm->active_config; + const struct ble_ll_cs_proc_params *params = &conf->proc_params; + uint32_t anchor_ticks; + uint8_t anchor_rem_usecs; + + ble_ll_conn_anchor_event_cntr_get(connsm, params->anchor_conn_event_cntr, + &anchor_ticks, &anchor_rem_usecs); + + ble_ll_tmr_add(&anchor_ticks, &anchor_rem_usecs, params->event_offset); + + if (anchor_ticks - g_ble_ll_sched_offset_ticks < ble_ll_tmr_get()) { + /* The start happend too late for the negotiated event counter. */ + return BLE_ERR_INV_LMP_LL_PARM; + } + + g_ble_ll_cs_sm_current = cssm; + cssm->anchor_usecs = ble_ll_tmr_t2u(anchor_ticks); + cssm->n_ap = aci_table[params->aci].n_ap; + cssm->procedure_anchor_usecs = cssm->anchor_usecs; + cssm->event_anchor_usecs = cssm->anchor_usecs; + cssm->subevent_anchor_usecs = cssm->anchor_usecs; + cssm->step_anchor_usecs = cssm->anchor_usecs; + cssm->current_step = NULL; + cssm->last_step = NULL; + + ble_ll_cs_proc_tifs_init(cssm->active_config->t_ip1, + cssm->active_config->t_ip2, + cssm->t_sw, + cssm->active_config->t_fcs); + + ble_ll_cs_proc_calculate_timing(cssm); + + cssm->mode0_next_chan_id = 0xFF; + cssm->non_mode0_next_chan_id = 0xFF; + + if (g_ble_ll_cs_local_cap.sounding_pct_estimate && + cssm->active_config->rtt_type != BLE_LL_CS_RTT_AA_ONLY) { + cssm->step_result.sounding_pct_estimate = 1; + } + + rc = ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); + if (rc) { + return BLE_ERR_UNSPECIFIED; + } + + return BLE_ERR_SUCCESS; +} + +int +ble_ll_cs_proc_scheduling_start(struct ble_ll_conn_sm *connsm, uint8_t config_id) +{ + int rc; + struct ble_ll_cs_sm *cssm = connsm->cssm; + struct ble_ll_cs_config *conf; + const struct ble_ll_cs_proc_params *params; + uint32_t anchor_ticks; + uint8_t anchor_rem_usecs; + + conf = &cssm->config[config_id]; + params = &conf->proc_params; + + /* Generating of the CS step configuration is time-consuming, so let's schedule + * the generation of the entire CS subevent in the connection event before + * the connection event that should start the CS procedure. + */ + ble_ll_conn_anchor_event_cntr_get(connsm, params->anchor_conn_event_cntr - 1, + &anchor_ticks, &anchor_rem_usecs); + + ble_ll_tmr_add(&anchor_ticks, &anchor_rem_usecs, params->event_offset); + + if (anchor_ticks < ble_ll_tmr_get()) { + /* The start happend too late for the negotiated event counter. */ + return BLE_ERR_INV_LMP_LL_PARM; + } + + cssm->active_config_id = config_id; + cssm->active_config = conf; + g_ble_ll_cs_sm_current = cssm; + + cssm->sched_cb = ble_ll_cs_proc_schedule_first_subevent; + cssm->sch.start_time = anchor_ticks; + cssm->sch.end_time = anchor_ticks + ble_ll_tmr_u2t_up(2100); + cssm->sch.remainder = 0; + cssm->sch.sched_type = BLE_LL_SCHED_TYPE_CS; + cssm->sch.cb_arg = cssm; + cssm->sch.sched_cb = ble_ll_cs_proc_sched_cb; + rc = ble_ll_sched_cs_proc(&cssm->sch); + if (rc) { + return BLE_ERR_UNSPECIFIED; + } + + return BLE_ERR_SUCCESS; +} + +void +ble_ll_cs_proc_sync_lost(struct ble_ll_cs_sm *cssm) +{ + ble_ll_cs_proc_set_now_as_anchor_point(cssm); + ble_phy_transition_set(BLE_PHY_TRANSITION_NONE, 0); + ble_phy_disable(); + ble_ll_state_set(BLE_LL_STATE_STANDBY); + /* TODO: Handle a lost sync */ +} + +/** + * Called when the wait for response timer expires while in the sync state. + * + * Context: Interrupt. + */ +void +ble_ll_cs_sync_wfr_timer_exp(void) +{ + struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current; + + BLE_LL_ASSERT(cssm != NULL); + + ble_ll_cs_proc_sync_lost(cssm); +} + +#endif /* BLE_LL_CHANNEL_SOUNDING */ diff --git a/nimble/controller/src/ble_ll_cs_sync.c b/nimble/controller/src/ble_ll_cs_sync.c new file mode 100644 index 0000000000..c69527ad2a --- /dev/null +++ b/nimble/controller/src/ble_ll_cs_sync.c @@ -0,0 +1,155 @@ +/* + * 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 +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include +#include "controller/ble_ll.h" +#include "controller/ble_ll_conn.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_tmr.h" +#include "controller/ble_hw.h" +#include "controller/ble_ll_hci.h" +#include "mbedtls/aes.h" +#include "ble_ll_cs_priv.h" + +extern struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap; +extern struct ble_ll_cs_sm *g_ble_ll_cs_sm_current; + +int +ble_ll_cs_rtt_generate(struct ble_ll_cs_drbg_ctx *drbg_ctx, struct ble_ll_cs_step *step, + uint8_t *buf, uint8_t *out_rtt_len, uint32_t buf_len, + uint16_t steps_in_procedure_count, uint8_t rtt_type, uint8_t role) +{ + int i; + int rc; + uint32_t aa; + uint8_t sequence_len = 0; + uint8_t bits; + + *out_rtt_len = 0; + + if (rtt_type != BLE_LL_CS_RTT_AA_ONLY && step->mode != BLE_LL_CS_MODE0) { + rc = ble_ll_cs_drbg_generate_sync_sequence(drbg_ctx, steps_in_procedure_count, + rtt_type, buf, &sequence_len); + + if (rc) { + return 0; + } + } + + /* Shift by 4 bits to make space for trailer bits */ + if (sequence_len > 0) { + /* If assert, increase the rtt_buffer or limit the CS subevent length */ + BLE_LL_ASSERT(sequence_len < buf_len && sequence_len < BLE_PHY_MAX_PDU_LEN); + + buf[sequence_len] = 0; + bits = 0; + for (i = sequence_len - 1; i >= 0; --i) { + bits = buf[i] >> 4; + buf[i] = (buf[i] << 4) & 0xF0; + buf[i + 1] = (buf[i + 1] & 0xF0) | bits; + } + } + + if (role == BLE_LL_CS_ROLE_INITIATOR) { + aa = step->initiator_aa; + } else { + aa = step->reflector_aa; + } + + /* Set the trailer bits */ + if (aa & 0x80000000) { + buf[0] |= 0x0A; + } else { + buf[0] |= 0x05; + } + ++sequence_len; + + *out_rtt_len = sequence_len; + + return 0; +} + +void +ble_ll_cs_sync_tx_end(struct ble_phy_cs_sync_results *results) +{ + struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current; + struct ble_phy_cs_transmission *transm; + uint32_t end_anchor_usecs; + + BLE_LL_ASSERT(cssm != NULL); + + transm = &cssm->current_step->next_transm->phy_transm; + + end_anchor_usecs = ble_ll_tmr_t2u(results->cputime) + results->rem_us; + cssm->step_result.time_of_departure_us = end_anchor_usecs; + cssm->step_result.time_of_departure_ns = results->rem_ns; + + cssm->anchor_usecs = end_anchor_usecs + transm->end_tifs; + ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); +} + +uint8_t +ble_ll_cs_rtt_tx_make(uint8_t *dptr, uint8_t *hdr_byte) +{ + struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current; + struct ble_ll_cs_step *step; + + BLE_LL_ASSERT(cssm != NULL); + + step = cssm->current_step; + memcpy(dptr, step->rtt_tx, step->rtt_tx_len); + *hdr_byte = 0; + + return step->rtt_tx_len; +} + +void +ble_ll_cs_sync_rx_end(struct ble_phy_cs_sync_results *results) +{ + struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current; + struct ble_phy_cs_transmission *transm; + uint32_t end_anchor_usecs; + + BLE_LL_ASSERT(cssm != NULL); + + transm = &cssm->current_step->next_transm->phy_transm; + end_anchor_usecs = ble_ll_tmr_t2u(results->cputime) + results->rem_us; + + cssm->step_result.time_of_arrival_us = end_anchor_usecs; + cssm->step_result.time_of_arrival_ns = results->rem_ns; + cssm->step_result.packet_rssi = results->rssi; + cssm->step_result.packet_quality = 0; + cssm->step_result.packet_nadm = 0xFF; + + if (g_ble_ll_cs_local_cap.sounding_pct_estimate && + cssm->active_config->rtt_type != BLE_LL_CS_RTT_AA_ONLY) { + /* TODO: Read PCT estimates from sounding sequence. + * For now set "Phase Correction Term is not available". + */ + cssm->step_result.packet_pct1 = 0xFFFFFFFF; + cssm->step_result.packet_pct2 = 0xFFFFFFFF; + } + + cssm->anchor_usecs = end_anchor_usecs + transm->end_tifs; + ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); +} + +#endif /* BLE_LL_CHANNEL_SOUNDING */ diff --git a/nimble/controller/src/ble_ll_cs_tone.c b/nimble/controller/src/ble_ll_cs_tone.c new file mode 100644 index 0000000000..f28329d22b --- /dev/null +++ b/nimble/controller/src/ble_ll_cs_tone.c @@ -0,0 +1,74 @@ +/* + * 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 +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include +#include "controller/ble_ll.h" +#include "controller/ble_ll_sched.h" +#include "controller/ble_ll_tmr.h" +#include "ble_ll_cs_priv.h" + +extern struct ble_ll_cs_sm *g_ble_ll_cs_sm_current; + +void +ble_ll_cs_tone_tx_end(struct ble_phy_cs_tone_results *results) +{ + struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current; + struct ble_phy_cs_transmission *transm; + struct ble_ll_cs_step *step; + uint8_t i; + + BLE_LL_ASSERT(cssm != NULL); + + step = cssm->current_step; + transm = &step->next_transm->phy_transm; + + if (step->mode == BLE_LL_CS_MODE0) { + /* TODO: Read measured frequency offset. + * For now set "Frequency offset is not available". + */ + cssm->step_result.measured_freq_offset = 0xC000; + } else if (step->mode == BLE_LL_CS_MODE2 || + step->mode == BLE_LL_CS_MODE3) { + for (i = 0; i < cssm->n_ap; ++i) { + cssm->step_result.tone_pct[i] = 0; + cssm->step_result.tone_quality_ind[i] = 0; + } + } + + cssm->anchor_usecs += transm->duration_usecs + transm->end_tifs; + ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); +} + +void +ble_ll_cs_tone_rx_end(struct ble_phy_cs_tone_results *results) +{ + struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current; + struct ble_phy_cs_transmission *transm; + + BLE_LL_ASSERT(cssm != NULL); + + transm = &cssm->current_step->next_transm->phy_transm; + + cssm->anchor_usecs += transm->duration_usecs + transm->end_tifs; + ble_ll_cs_proc_schedule_next_tx_or_rx(cssm); +} + +#endif /* BLE_LL_CHANNEL_SOUNDING */ diff --git a/nimble/controller/src/ble_ll_ctrl.c b/nimble/controller/src/ble_ll_ctrl.c index a05b5c9d8b..a3736c1972 100644 --- a/nimble/controller/src/ble_ll_ctrl.c +++ b/nimble/controller/src/ble_ll_ctrl.c @@ -49,6 +49,10 @@ #include "console/console.h" #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +#include "controller/ble_ll_cs.h" +#endif + /* * XXX: * 1) Do I need to keep track of which procedures have already been done? @@ -128,6 +132,23 @@ const uint8_t g_ble_ll_ctrl_pkt_lengths[BLE_LL_CTRL_OPCODES] = BLE_LL_CTRL_SUBRATE_IND_LEN, BLE_LL_CTRL_CHAN_REPORTING_IND_LEN, BLE_LL_CTRL_CHAN_STATUS_IND_LEN, + BLE_LL_CTRL_PERIODIC_SYNC_WR_IND_LEN, + BLE_LL_CTRL_FEATURE_EXT_REQ_LEN, + BLE_LL_CTRL_FEATURE_EXT_RSP_LEN, + BLE_LL_CTRL_CS_SEC_RSP_LEN, + BLE_LL_CTRL_CS_CAPABILITIES_REQ_LEN, + BLE_LL_CTRL_CS_CAPABILITIES_RSP_LEN, + BLE_LL_CTRL_CS_CONFIG_REQ_LEN, + BLE_LL_CTRL_CS_CONFIG_RSP_LEN, + BLE_LL_CTRL_CS_REQ_LEN, + BLE_LL_CTRL_CS_RSP_LEN, + BLE_LL_CTRL_CS_IND_LEN, + BLE_LL_CTRL_CS_TERMINATE_REQ_LEN, + BLE_LL_CTRL_CS_FAE_REQ_LEN, + BLE_LL_CTRL_CS_FAE_RSP_LEN, + BLE_LL_CTRL_CS_CHANNEL_MAP_IND_LEN, + BLE_LL_CTRL_CS_SEC_REQ_LEN, + BLE_LL_CTRL_CS_TERMINATE_RSP_LEN }; /** @@ -2045,6 +2066,26 @@ ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr, ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_SUBRATE_UPDATE); break; #endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_CTRL_PROC_CS_CAP_XCHG: + ble_ll_cs_rx_capabilities_req_rejected(connsm, ble_error); + break; + case BLE_LL_CTRL_PROC_CS_FAE_REQ: + ble_ll_cs_rx_fae_req_rejected(connsm, ble_error); + break; + case BLE_LL_CTRL_PROC_CS_CONF: + ble_ll_cs_rx_config_req_rejected(connsm, ble_error); + break; + case BLE_LL_CTRL_PROC_CS_SEC_START: + ble_ll_cs_rx_security_req_rejected(connsm, ble_error); + break; + case BLE_LL_CTRL_PROC_CS_START: + ble_ll_cs_rx_cs_start_rejected(connsm, ble_error); + break; + case BLE_LL_CTRL_PROC_CS_TERMINATE: + ble_ll_cs_rx_cs_terminate_req_rejected(connsm, ble_error); + break; +#endif default: break; @@ -2567,6 +2608,34 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc, void *data) &connsm->subrate_trans); break; #endif +#endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_CTRL_PROC_CS_CAP_XCHG: + opcode = BLE_LL_CTRL_CS_CAPABILITIES_REQ; + ble_ll_cs_capabilities_pdu_make(connsm, ctrdata); + break; + case BLE_LL_CTRL_PROC_CS_FAE_REQ: + opcode = BLE_LL_CTRL_CS_FAE_REQ; + /* No command parameters in LL_CS_FAE_REQ PDU */ + break; + case BLE_LL_CTRL_PROC_CS_CONF: + opcode = BLE_LL_CTRL_CS_CONFIG_REQ; + ble_ll_cs_config_req_make(connsm, ctrdata); + break; +#if MYNEWT_VAL(BLE_LL_ROLE_CENTRAL) + case BLE_LL_CTRL_PROC_CS_SEC_START: + opcode = BLE_LL_CTRL_CS_SEC_REQ; + ble_ll_cs_security_req_make(connsm, ctrdata); + break; +#endif + case BLE_LL_CTRL_PROC_CS_START: + opcode = BLE_LL_CTRL_CS_REQ; + ble_ll_cs_start_req_make(connsm, ctrdata); + break; + case BLE_LL_CTRL_PROC_CS_TERMINATE: + opcode = BLE_LL_CTRL_CS_TERMINATE_REQ; + ble_ll_cs_terminate_req_make(connsm, ctrdata); + break; #endif default: BLE_LL_ASSERT(0); @@ -3037,6 +3106,53 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om) case BLE_LL_CTRL_SUBRATE_IND: rsp_opcode = ble_ll_ctrl_rx_subrate_ind(connsm, dptr, rspdata); break; +#endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_CTRL_CS_CAPABILITIES_REQ: + rsp_opcode = ble_ll_cs_rx_capabilities_req(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_CS_CAPABILITIES_RSP: + ble_ll_cs_rx_capabilities_rsp(connsm, dptr); + break; + case BLE_LL_CTRL_CS_FAE_REQ: + rsp_opcode = ble_ll_cs_rx_fae_req(connsm, om); + break; + case BLE_LL_CTRL_CS_FAE_RSP: + ble_ll_cs_rx_fae_rsp(connsm, dptr); + break; + case BLE_LL_CTRL_CS_CONFIG_REQ: + rsp_opcode = ble_ll_cs_rx_config_req(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_CS_CONFIG_RSP: + ble_ll_cs_rx_config_rsp(connsm, dptr); + break; +#if MYNEWT_VAL(BLE_LL_ROLE_PERIPHERAL) + case BLE_LL_CTRL_CS_SEC_REQ: + rsp_opcode = ble_ll_cs_rx_security_req(connsm, dptr, rspdata); + break; +#endif +#if MYNEWT_VAL(BLE_LL_ROLE_CENTRAL) + case BLE_LL_CTRL_CS_SEC_RSP: + ble_ll_cs_rx_security_rsp(connsm, dptr); + break; +#endif + case BLE_LL_CTRL_CS_REQ: + rsp_opcode = ble_ll_cs_rx_cs_start_req(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_CS_RSP: + rsp_opcode = ble_ll_cs_rx_cs_start_rsp(connsm, dptr, rspdata); + break; +#if MYNEWT_VAL(BLE_LL_ROLE_PERIPHERAL) + case BLE_LL_CTRL_CS_IND: + rsp_opcode = ble_ll_cs_rx_cs_start_ind(connsm, dptr, rspdata); + break; +#endif + case BLE_LL_CTRL_CS_TERMINATE_REQ: + rsp_opcode = ble_ll_cs_rx_cs_terminate_req(connsm, dptr, rspdata); + break; + case BLE_LL_CTRL_CS_TERMINATE_RSP: + ble_ll_cs_rx_cs_terminate_rsp(connsm, dptr); + break; #endif default: /* Nothing to do here */ diff --git a/nimble/controller/src/ble_ll_sched.c b/nimble/controller/src/ble_ll_sched.c index 8f4599bdf3..8e37b0fe48 100644 --- a/nimble/controller/src/ble_ll_sched.c +++ b/nimble/controller/src/ble_ll_sched.c @@ -39,6 +39,11 @@ #include "ble_ll_priv.h" #include "ble_ll_conn_priv.h" +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +void ble_ll_cs_proc_rm_from_sched(void *cb_arg); +void ble_ll_cs_proc_halt(void); +#endif + #define BLE_LL_SCHED_MAX_DELAY_ANY (0x7fffffff) static struct ble_ll_tmr g_ble_ll_sched_timer; @@ -183,9 +188,14 @@ ble_ll_sched_preempt(struct ble_ll_sched_item *sch, ble_ll_ext_sched_removed(entry); break; #endif - default: - BLE_LL_ASSERT(0); - break; +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_SCHED_TYPE_CS: + ble_ll_cs_proc_rm_from_sched(entry->cb_arg); + break; +#endif + default: + BLE_LL_ASSERT(0); + break; } entry = next; @@ -865,6 +875,25 @@ ble_ll_sched_iso_big(struct ble_ll_sched_item *sch, int first, int fixed) } #endif /* BLE_LL_ISO_BROADCASTER */ +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) +int +ble_ll_sched_cs_proc(struct ble_ll_sched_item *sch) +{ + os_sr_t sr; + int rc; + + OS_ENTER_CRITICAL(sr); + + rc = ble_ll_sched_insert(sch, 0, preempt_none); + + OS_EXIT_CRITICAL(sr); + + ble_ll_sched_restart(); + + return rc; +} +#endif /* BLE_LL_CHANNEL_SOUNDING */ + /** * Remove a schedule element * @@ -1014,6 +1043,11 @@ ble_ll_sched_execute_item(struct ble_ll_sched_item *sch) case BLE_LL_STATE_EXTERNAL: ble_ll_ext_halt(); break; +#endif +#if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING) + case BLE_LL_STATE_CS: + ble_ll_cs_proc_halt(); + break; #endif default: BLE_LL_ASSERT(0); diff --git a/nimble/controller/syscfg.yml b/nimble/controller/syscfg.yml index 1e830dbecb..a76451a9e6 100644 --- a/nimble/controller/syscfg.yml +++ b/nimble/controller/syscfg.yml @@ -569,6 +569,10 @@ syscfg.defs: - (BLE_VERSION >= 54) if 1 value: MYNEWT_VAL(BLE_CHANNEL_SOUNDING) state: experimental + BLE_LL_CHANNEL_SOUNDING_SUBEVENT_EV_MAX_CNT: + description: > + The maximum number of queued HCI Subevent Results (Continue) events. + value: 5 BLE_LL_SYSINIT_STAGE: description: > diff --git a/nimble/controller/test/src/ble_ll_cs_drbg_test.c b/nimble/controller/test/src/ble_ll_cs_drbg_test.c new file mode 100644 index 0000000000..1edb920713 --- /dev/null +++ b/nimble/controller/test/src/ble_ll_cs_drbg_test.c @@ -0,0 +1,369 @@ +/* + * 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 +#include +#include +#include "ble_ll_cs_drbg_priv.h" + +extern uint8_t g_ble_ll_cs_chan_indices[72]; + +static void +ble_ll_cs_drbg_e_test(void) +{ + uint8_t key[16] = {0}; + uint8_t data[16] = {0}; + uint8_t out[16] = {0}; + uint8_t expected_out[16] = {0}; + + /* Sample data from BT spec Vol 6, Part C, 1.1 Encrypt Command + * Swap because the copy-pasted strings are in leftmost (MSO) to rightmost + * (LSO) orientation. + */ + swap_buf(key, (uint8_t [16]) { + 0x4C, 0x68, 0x38, 0x41, 0x39, 0xF5, 0x74, 0xD8, + 0x36, 0xBC, 0xF3, 0x4E, 0x9D, 0xFB, 0x01, 0xBF + }, 16); + + swap_buf(data, (uint8_t [16]) { + 0x02, 0x13, 0x24, 0x35, 0x46, 0x57, 0x68, 0x79, + 0xac, 0xbd, 0xce, 0xdf, 0xe0, 0xf1, 0x02, 0x13 + }, 16); + + swap_buf(expected_out, (uint8_t [16]) { + 0x99, 0xad, 0x1b, 0x52, 0x26, 0xa3, 0x7e, 0x3e, + 0x05, 0x8e, 0x3b, 0x8e, 0x27, 0xc2, 0xc6, 0x66 + }, 16); + + ble_ll_cs_drbg_e(key, data, out); + TEST_ASSERT(memcmp(out, expected_out, sizeof(out)) == 0); +} + +static void +ble_ll_cs_drbg_f7_test(void) +{ + uint8_t v_s[80]; + uint8_t k[16]; + uint8_t k2[16] = {0}; + uint8_t x[16] = {0}; + uint8_t expected_k2[16] = {0}; + uint8_t expected_x[16] = {0}; + + /* Sample data from BT spec Vol 6, Part C, 7. Deterministic + * random bit generator sample data. + * Swap because the copy-pasted strings are in leftmost (MSO) + * to rightmost (LSO) orientation. + */ + swap_buf(k, (uint8_t [16]) { + 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, + 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F + }, 16); + + swap_buf(v_s, (uint8_t [80]) { + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x28, 0x00, 0x00, 0x00, 0x20, + 0xE1, 0x0B, 0xC2, 0x8A, 0x0B, 0xFD, 0xDF, 0xE9, + 0x3E, 0x7F, 0x51, 0x86, 0xE0, 0xCA, 0x0B, 0x3B, + 0x9F, 0xF4, 0x77, 0xC1, 0x86, 0x73, 0x84, 0x0D, + 0xC9, 0x80, 0xDE, 0xDF, 0x98, 0x82, 0xED, 0x44, + 0x64, 0xA6, 0x74, 0x96, 0x78, 0x68, 0xF1, 0x43, + 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 + }, 80); + + swap_buf(expected_k2, (uint8_t [16]) { + 0x8B, 0x2B, 0x06, 0xDC, 0x52, 0x2D, 0x3E, 0x0A, + 0xF0, 0xA5, 0x0C, 0xAF, 0x48, 0x10, 0xE0, 0x35 + }, 16); + + TEST_ASSERT(ble_ll_cs_drbg_f7(k, v_s, sizeof(v_s), k2) == 0); + TEST_ASSERT(memcmp(k2, expected_k2, sizeof(k2)) == 0); + + v_s[76] = 0x01; + swap_buf(expected_x, (uint8_t [16]) { + 0xA3, 0x4F, 0xBE, 0x57, 0xF8, 0xF9, 0x7E, 0x34, + 0x9D, 0x15, 0xA3, 0x76, 0x79, 0x60, 0x74, 0x64 + }, 16); + + TEST_ASSERT(ble_ll_cs_drbg_f7(k, v_s, sizeof(v_s), x) == 0); + TEST_ASSERT(memcmp(x, expected_x, sizeof(x)) == 0); +} + +static void +ble_ll_cs_drbg_f8_test(void) +{ + uint8_t input_bit_string[40] = {0}; + uint8_t expected_sm[32] = {0}; + uint8_t sm[32] = {0}; + + /* Sample data from BT spec Vol 6, Part C, 7. Deterministic + * random bit generator sample data. + * Swap because the copy-pasted strings are in leftmost (MSO) + * to rightmost (LSO) orientation. + */ + + /* 320-bit input bit string created from concatenated vectors + * CS_IV || CS_IN || CS_PV + */ + swap_buf(input_bit_string, (uint8_t [40]) { + 0xE1, 0x0B, 0xC2, 0x8A, 0x0B, 0xFD, 0xDF, 0xE9, + 0x3E, 0x7F, 0x51, 0x86, 0xE0, 0xCA, 0x0B, 0x3B, + 0x9F, 0xF4, 0x77, 0xC1, 0x86, 0x73, 0x84, 0x0D, + 0xC9, 0x80, 0xDE, 0xDF, 0x98, 0x82, 0xED, 0x44, + 0x64, 0xA6, 0x74, 0x96, 0x78, 0x68, 0xF1, 0x43 + }, 40); + + swap_buf(expected_sm, (uint8_t [32]) { + 0xB6, 0x02, 0xB1, 0xB2, 0x8C, 0x6F, 0x0A, 0x3D, + 0xDA, 0xE6, 0x37, 0xB4, 0x84, 0x25, 0x08, 0x7D, + 0xDC, 0x18, 0x8C, 0x89, 0xA1, 0xB0, 0xCD, 0xFD, + 0xA1, 0xE8, 0xFC, 0x66, 0xC9, 0x99, 0x97, 0x50 + }, 32); + + TEST_ASSERT(ble_ll_cs_drbg_f8(input_bit_string, sm) == 0); + TEST_ASSERT(memcmp(sm, expected_sm, sizeof(sm)) == 0); +} + +static void +ble_ll_cs_drbg_f9_test(void) +{ + uint8_t sm[32] = {0}; + uint8_t k[16] = {0}; + uint8_t v[16] = {0}; + uint8_t expected_k[16] = {0}; + uint8_t expected_v[16] = {0}; + + /* First call to f9 from instantiation function h9, + * K and V vectors filled with zeros. + * + * Sample data from BT spec Vol 6, Part C, 7. Deterministic + * random bit generator sample data. + * Swap because the copy-pasted strings are in leftmost (MSO) + * to rightmost (LSO) orientation. + */ + + swap_buf(sm, (uint8_t [32]) { + 0xB6, 0x02, 0xB1, 0xB2, 0x8C, 0x6F, 0x0A, 0x3D, + 0xDA, 0xE6, 0x37, 0xB4, 0x84, 0x25, 0x08, 0x7D, + 0xDC, 0x18, 0x8C, 0x89, 0xA1, 0xB0, 0xCD, 0xFD, + 0xA1, 0xE8, 0xFC, 0x66, 0xC9, 0x99, 0x97, 0x50 + }, 32); + + swap_buf(expected_k, (uint8_t [16]) { + 0xEE, 0xE0, 0x4D, 0x7C, 0x76, 0x11, 0x3A, 0x5C, + 0xEC, 0x99, 0x2A, 0xE3, 0x20, 0xC2, 0x4D, 0x27 + }, 16); + + swap_buf(expected_v, (uint8_t [16]) { + 0xDF, 0x90, 0x56, 0x47, 0xC1, 0x06, 0x6E, 0x6F, + 0x52, 0xC0, 0x3E, 0xDF, 0xB8, 0x2B, 0x69, 0x28 + }, 16); + + TEST_ASSERT(ble_ll_cs_drbg_f9(sm, k, v) == 0); + TEST_ASSERT(memcmp(k, expected_k, sizeof(k)) == 0); + TEST_ASSERT(memcmp(v, expected_v, sizeof(v)) == 0); +} + +static void +cs_drbg_init(struct ble_ll_cs_drbg_ctx *ctx) +{ + memset(ctx, 0, sizeof(*ctx)); + + /* CS_IV = CS_IV_P || CS_IV_C */ + swap_buf(ctx->iv, (uint8_t [16]) { + 0xE1, 0x0B, 0xC2, 0x8A, 0x0B, 0xFD, 0xDF, 0xE9, + 0x3E, 0x7F, 0x51, 0x86, 0xE0, 0xCA, 0x0B, 0x3B + }, 16); + + /* CS_IN = CS_IN_P || CS_IN_C */ + swap_buf(ctx->in, (uint8_t [8]) { + 0x9F, 0xF4, 0x77, 0xC1, 0x86, 0x73, 0x84, 0x0D + }, 8); + + /* CS_PV = CS_PV_P || CS_PV_C */ + swap_buf(ctx->pv, (uint8_t [16]) { + 0xC9, 0x80, 0xDE, 0xDF, 0x98, 0x82, 0xED, 0x44, + 0x64, 0xA6, 0x74, 0x96, 0x78, 0x68, 0xF1, 0x43 + }, 16); + + ble_ll_cs_drbg_init(ctx); +} + +static void +ble_ll_cs_drbg_rand_test(void) +{ + struct ble_ll_cs_drbg_ctx ctx; + uint8_t output[20] = {0}; + uint8_t expected_output[20] = {0}; + + /* Test if subsequent drgb generator calls returns expected bit sequences. */ + + cs_drbg_init(&ctx); + + /* First round - request full 128-bit batch */ + swap_buf(expected_output, (uint8_t [16]) { + 0x79, 0x74, 0x1F, 0xD1, 0x8F, 0x57, 0x7B, 0x45, + 0xD0, 0x9A, 0x66, 0x5A, 0x7F, 0x1F, 0x28, 0x58 + }, 16); + + TEST_ASSERT(ble_ll_cs_drbg_rand( + &ctx, 0x00, BLE_LL_CS_DRBG_HOP_CHAN_NON_MODE0, + output, 16) == 0); + TEST_ASSERT(memcmp(output, expected_output, 16) == 0); +} + +static void +ble_ll_cs_drbg_chan_selection_3b_test(void) +{ + struct ble_ll_cs_drbg_ctx ctx; + uint8_t filtered_channels[72] = {0}; + uint8_t shuffled_channels[72] = {0}; + uint8_t expected_shuffled_channels[19] = {0}; + + cs_drbg_init(&ctx); + + memcpy(filtered_channels, (uint8_t [19]) { + 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, + 13, 14, 15, 16, 17, 18, 19, 20 + }, 19); + + memcpy(expected_shuffled_channels, (uint8_t [19]) { + 11, 7, 14, 18, 9, 19, 10, 8, 5, 2, 4, 15, 16, 13, 12, 6, 17, 20, 3 + }, 19); + assert(ble_ll_cs_drbg_shuffle_cr1(&ctx, 0x00, BLE_LL_CS_DRBG_HOP_CHAN_MODE0, + filtered_channels, shuffled_channels, 19) == 0); + assert(memcmp(shuffled_channels, expected_shuffled_channels, 19) == 0); + + memcpy(expected_shuffled_channels, (uint8_t [19]) { + 6, 12, 5, 10, 3, 2, 18, 17, 16, 8, 11, 7, 19, 4, 13, 20, 9, 15, 14 + }, 19); + assert(ble_ll_cs_drbg_shuffle_cr1(&ctx, 0x03, BLE_LL_CS_DRBG_HOP_CHAN_NON_MODE0, + filtered_channels, shuffled_channels, 19) == 0); + assert(memcmp(shuffled_channels, expected_shuffled_channels, 19) == 0); +} + +static void +ble_ll_cs_drbg_generate_aa_test(void) +{ + struct ble_ll_cs_drbg_ctx ctx; + uint32_t initiator_aa; + uint32_t reflector_aa; + uint32_t expected_initiator_aa; + uint32_t expected_reflector_aa; + + cs_drbg_init(&ctx); + + /* Step 0 */ + assert(ble_ll_cs_drbg_generate_aa(&ctx, 0, &initiator_aa, &reflector_aa) == 0); + expected_initiator_aa = get_be32((uint8_t [4]) {0x6C, 0x37, 0x6A, 0xB8}); + expected_reflector_aa = get_be32((uint8_t [4]) {0xF0, 0x79, 0xBC, 0x3A}); + assert(initiator_aa == expected_initiator_aa); + assert(reflector_aa == expected_reflector_aa); + + /* Step 1 */ + assert(ble_ll_cs_drbg_generate_aa(&ctx, 1, &initiator_aa, &reflector_aa) == 0); + expected_initiator_aa = get_be32((uint8_t [4]) {0x01, 0x1C, 0xAE, 0x4E}); + expected_reflector_aa = get_be32((uint8_t [4]) {0xD0, 0x6A, 0xCD, 0xDA}); + assert(initiator_aa == expected_initiator_aa); + assert(reflector_aa == expected_reflector_aa); + + /* Step 2 */ + assert(ble_ll_cs_drbg_generate_aa(&ctx, 2, &initiator_aa, &reflector_aa) == 0); + expected_initiator_aa = get_be32((uint8_t [4]) {0x64, 0x06, 0x12, 0x14}); + expected_reflector_aa = get_be32((uint8_t [4]) {0x28, 0x94, 0x2F, 0x38}); + assert(initiator_aa == expected_initiator_aa); + assert(reflector_aa == expected_reflector_aa); + + /* Step 14 */ + assert(ble_ll_cs_drbg_generate_aa(&ctx, 14, &initiator_aa, &reflector_aa) == 0); + expected_initiator_aa = get_be32((uint8_t [4]) {0xF7, 0x21, 0x97, 0x86}); + expected_reflector_aa = get_be32((uint8_t [4]) {0x57, 0x17, 0x64, 0x70}); + assert(initiator_aa == expected_initiator_aa); + assert(reflector_aa == expected_reflector_aa); +} + +static void +ble_ll_cs_drbg_rand_marker_position_test(void) +{ + uint8_t position1; + uint8_t position2; + struct ble_ll_cs_drbg_ctx ctx; + + cs_drbg_init(&ctx); + + /* Step 9 */ + assert(ble_ll_cs_drbg_rand_marker_position( + &ctx, 9, BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE, + &position1, &position2) == 0); + assert(position1 == 12 && position2 == 0xFF); + + assert(ble_ll_cs_drbg_rand_marker_position( + &ctx, 9, BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE, + &position1, &position2) == 0); + assert(position1 == 4 && position2 == 0xFF); + + /* Step 14 */ + assert(ble_ll_cs_drbg_rand_marker_position( + &ctx, 14, BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE, + &position1, &position2) == 0); + assert(position1 == 8 && position2 == 0xFF); + + assert(ble_ll_cs_drbg_rand_marker_position( + &ctx, 14, BLE_LL_CS_RTT_32_BIT_SOUNDING_SEQUENCE, + &position1, &position2) == 0); + assert(position1 == 11 && position2 == 0xFF); +} + +static void +ble_ll_cs_drbg_rand_marker_selection_test(void) +{ + uint8_t marker_selection; + struct ble_ll_cs_drbg_ctx ctx; + + cs_drbg_init(&ctx); + + /* Step 9 */ + assert(ble_ll_cs_drbg_rand_marker_selection(&ctx, 0x09, &marker_selection) == 0); + assert(marker_selection == 0x00); + + assert(ble_ll_cs_drbg_rand_marker_selection(&ctx, 0x09, &marker_selection) == 0); + assert(marker_selection == 0x80); + + memset(ctx.t_cache, 0, sizeof(ctx.t_cache)); + + /* Step 14 */ + assert(ble_ll_cs_drbg_rand_marker_selection(&ctx, 14, &marker_selection) == 0); + assert(marker_selection == 0x80); + + assert(ble_ll_cs_drbg_rand_marker_selection(&ctx, 14, &marker_selection) == 0); + assert(marker_selection == 0x80); +} + +TEST_SUITE(ble_ll_cs_drbg_test_suite) { + ble_ll_cs_drbg_e_test(); + ble_ll_cs_drbg_f7_test(); + ble_ll_cs_drbg_f8_test(); + ble_ll_cs_drbg_f9_test(); + ble_ll_cs_drbg_rand_test(); + ble_ll_cs_drbg_chan_selection_3b_test(); + ble_ll_cs_drbg_generate_aa_test(); + ble_ll_cs_drbg_rand_marker_position_test(); + ble_ll_cs_drbg_rand_marker_selection_test(); +} diff --git a/nimble/controller/test/src/ble_ll_test.c b/nimble/controller/test/src/ble_ll_test.c index 818430c577..93e472db38 100644 --- a/nimble/controller/test/src/ble_ll_test.c +++ b/nimble/controller/test/src/ble_ll_test.c @@ -27,6 +27,7 @@ TEST_SUITE_DECL(ble_ll_crypto_test_suite); TEST_SUITE_DECL(ble_ll_csa2_test_suite); TEST_SUITE_DECL(ble_ll_isoal_test_suite); TEST_SUITE_DECL(ble_ll_iso_test_suite); +TEST_SUITE_DECL(ble_ll_cs_drbg_test_suite); int main(int argc, char **argv) @@ -36,6 +37,7 @@ main(int argc, char **argv) ble_ll_csa2_test_suite(); ble_ll_isoal_test_suite(); ble_ll_iso_test_suite(); + ble_ll_cs_drbg_test_suite(); return tu_any_failed; } diff --git a/nimble/controller/test/syscfg.yml b/nimble/controller/test/syscfg.yml index f7a3c4ef5e..cf6a5de8e6 100644 --- a/nimble/controller/test/syscfg.yml +++ b/nimble/controller/test/syscfg.yml @@ -27,3 +27,7 @@ syscfg.vals: NATIVE_SOCKETS_PRIO: 3 BLE_TRANSPORT_ISO_SIZE: 255 + + MBEDTLS_AES_C: 1 + BLE_CHANNEL_SOUNDING: 1 + BLE_LL_CHANNEL_SOUNDING: 1 diff --git a/nimble/host/include/host/ble_cs.h b/nimble/host/include/host/ble_cs.h index 3abbd11e65..e4ed025c81 100644 --- a/nimble/host/include/host/ble_cs.h +++ b/nimble/host/include/host/ble_cs.h @@ -31,6 +31,7 @@ struct ble_cs_event { { struct { + uint32_t time_diff_ns; uint16_t conn_handle; uint8_t status; } procedure_complete; diff --git a/nimble/host/src/ble_cs.c b/nimble/host/src/ble_cs.c index cef622206b..5ee4cb23af 100644 --- a/nimble/host/src/ble_cs.c +++ b/nimble/host/src/ble_cs.c @@ -31,6 +31,44 @@ #include "sys/queue.h" #include "ble_hs_hci_priv.h" +#define BLE_HS_CS_MODE0 (0) +#define BLE_HS_CS_MODE1 (1) +#define BLE_HS_CS_MODE2 (2) +#define BLE_HS_CS_MODE3 (3) +#define BLE_HS_CS_MODE_UNUSED (0xff) +#define BLE_HS_CS_SUBMODE_TYPE BLE_HS_CS_MODE_UNUSED + +#define BLE_HS_CS_ROLE_INITIATOR (0) +#define BLE_HS_CS_ROLE_REFLECTOR (1) + +#define BLE_HS_CS_RTT_AA_ONLY (0x00) +#define BLE_HS_CS_RTT_32_BIT_SOUNDING_SEQUENCE (0x01) +#define BLE_HS_CS_RTT_96_BIT_SOUNDING_SEQUENCE (0x02) +#define BLE_HS_CS_RTT_32_BIT_RANDOM_SEQUENCE (0x03) +#define BLE_HS_CS_RTT_64_BIT_RANDOM_SEQUENCE (0x04) +#define BLE_HS_CS_RTT_96_BIT_RANDOM_SEQUENCE (0x05) +#define BLE_HS_CS_RTT_128_BIT_RANDOM_SEQUENCE (0x06) + +#define BLE_HS_CS_SUBEVENT_DONE_STATUS_COMPLETED (0x0) +#define BLE_HS_CS_SUBEVENT_DONE_STATUS_PARTIAL (0x1) +#define BLE_HS_CS_SUBEVENT_DONE_STATUS_ABORTED (0xF) + +#define BLE_HS_CS_PROC_DONE_STATUS_COMPLETED (0x0) +#define BLE_HS_CS_PROC_DONE_STATUS_PARTIAL (0x1) +#define BLE_HS_CS_PROC_DONE_STATUS_ABORTED (0xF) + +#define TIME_DIFF_NOT_AVAILABLE (0x00008000) + +#define IN_RANGE(_n, _min, _max) (((_n) >= (_min)) && ((_n) <= (_max))) +#define N_AP_MAX (4) + +static uint8_t aci_table[] = {1, 2, 3, 4, 2, 3, 4, 4}; +static uint8_t local_role; +static uint8_t sounding_pct_estimate; +static uint8_t rtt_type; +static int32_t time_diff_sum; +static uint32_t time_diff_count; + struct ble_cs_rd_rem_supp_cap_cp { uint16_t conn_handle; } __attribute__((packed)); @@ -115,7 +153,6 @@ struct ble_cs_create_config_cp { uint8_t channel_selection_type; uint8_t ch3c_shape; uint8_t ch3c_jump; - uint8_t companion_signal_enable; } __attribute__((packed)); struct ble_cs_remove_config_cp { @@ -189,7 +226,8 @@ ble_cs_call_event_cb(struct ble_cs_event *event) } static void -ble_cs_call_procedure_complete_cb(uint16_t conn_handle, uint8_t status) +ble_cs_call_procedure_complete_cb(uint16_t conn_handle, uint8_t status, + uint32_t time_diff_ns) { struct ble_cs_event event; @@ -197,6 +235,7 @@ ble_cs_call_procedure_complete_cb(uint16_t conn_handle, uint8_t status) event.type = BLE_CS_EVENT_CS_PROCEDURE_COMPLETE; event.procedure_complete.conn_handle = conn_handle; event.procedure_complete.status = status; + event.procedure_complete.time_diff_ns = time_diff_ns; ble_cs_call_event_cb(&event); } @@ -218,7 +257,8 @@ ble_cs_rd_loc_supp_cap(void) rp.optional_t_ip2_times_supported = le16toh(rp.optional_t_ip2_times_supported); rp.optional_t_fcs_times_supported = le16toh(rp.optional_t_fcs_times_supported); rp.optional_t_pm_times_supported = le16toh(rp.optional_t_pm_times_supported); - (void) rp; + + sounding_pct_estimate = (rp.optional_subfeatures_supported >> 3) & 1; return rc; } @@ -361,7 +401,7 @@ ble_cs_create_config(const struct ble_cs_create_config_cp *cmd) cp.channel_selection_type = cmd->channel_selection_type; cp.ch3c_shape = cmd->ch3c_shape; cp.ch3c_jump = cmd->ch3c_jump; - cp.companion_signal_enable = cmd->companion_signal_enable; + cp.reserved = 0x00; return ble_hs_hci_cmd_tx(BLE_HCI_OP(BLE_HCI_OGF_LE, BLE_HCI_OCF_LE_CS_CREATE_CONFIG), @@ -505,12 +545,17 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, /* Measure phase rotations in main mode */ cmd.main_mode_type = 0x01; /* Do not use sub mode for now. */ - cmd.sub_mode_type = 0xFF; - /* Range from which the number of CS main mode steps to execute - * will be randomly selected. - */ - cmd.min_main_mode_steps = 0x02; - cmd.max_main_mode_steps = 0x06; + cmd.sub_mode_type = BLE_HS_CS_SUBMODE_TYPE; + if (cmd.sub_mode_type == BLE_HS_CS_MODE_UNUSED) { + cmd.min_main_mode_steps = 0; + cmd.max_main_mode_steps = 0; + } else { + /* Range from which the number of CS main mode steps to execute + * will be randomly selected. + */ + cmd.min_main_mode_steps = 0x02; + cmd.max_main_mode_steps = 0x06; + } /* The number of main mode steps to be repeated at the beginning of * the current CS, irrespectively if there are some overlapping main * mode steps from previous CS subevent or not. @@ -521,18 +566,20 @@ ble_hs_hci_evt_le_cs_rd_rem_fae_complete(uint8_t subevent, const void *data, */ cmd.mode_0_steps = 0x03; /* Take the Initiator role */ - cmd.role = 0x00; - cmd.rtt_type = 0x01; + local_role = BLE_HS_CS_ROLE_INITIATOR; + cmd.role = local_role; + + rtt_type = BLE_HS_CS_RTT_32_BIT_SOUNDING_SEQUENCE; + cmd.rtt_type = rtt_type; + cmd.cs_sync_phy = 0x01; - memcpy(cmd.channel_map, (uint8_t[10]) {0x0a, 0xfa, 0xcf, 0xac, 0xfa, 0xc0}, 10); + memcpy(cmd.channel_map, (uint8_t[10]) {0xFC, 0xFF, 0x0F}, 10); cmd.channel_map_repetition = 0x01; /* Use Channel Selection Algorithm #3b */ cmd.channel_selection_type = 0x00; /* Ignore these as used only with #3c algorithm */ cmd.ch3c_shape = 0x00; cmd.ch3c_jump = 0x00; - /* EDLC/ECLD attack protection not supported */ - cmd.companion_signal_enable = 0x00; /* Create CS config */ rc = ble_cs_create_config(&cmd); @@ -564,7 +611,7 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, cmd.conn_handle = le16toh(ev->conn_handle); cmd.config_id = 0x00; /* The maximum duration of each CS procedure (time = N × 0.625 ms) */ - cmd.max_procedure_len = 8; + cmd.max_procedure_len = 800; /* The maximum number of consecutive CS procedures to be scheduled * as part of this measurement */ @@ -577,8 +624,8 @@ ble_hs_hci_evt_le_cs_sec_enable_complete(uint8_t subevent, const void *data, /* Minimum/maximum suggested durations for each CS subevent in microseconds. * 1250us and 5000us selected. */ - cmd.min_subevent_len = 1250; - cmd.max_subevent_len = 5000; + cmd.min_subevent_len = 10000; + cmd.max_subevent_len = 10000; /* Use ACI 0 as we have only one antenna on each side */ cmd.tone_antenna_config_selection = 0x00; /* Use LE 1M PHY for CS procedures */ @@ -628,7 +675,7 @@ ble_hs_hci_evt_le_cs_config_complete(uint8_t subevent, const void *data, rc = ble_cs_sec_enable(&cmd); if (rc) { BLE_HS_LOG(DEBUG, "Failed to enable CS security"); - ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), ev->status); + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), ev->status, 0); } return 0; @@ -647,16 +694,248 @@ ble_hs_hci_evt_le_cs_proc_enable_complete(uint8_t subevent, const void *data, return 0; } +static int +ble_cs_add_mode0_result(const uint8_t *data, uint8_t data_len) +{ + uint16_t measured_freq_offset; + uint8_t packet_quality; + uint8_t packet_rssi; + uint8_t cs_sync_antenna; + + if (!IN_RANGE(data_len, 3, 5)) { + /* Ignore invalid formatted results */ + return 1; + } + + packet_quality = data[0]; + packet_rssi = data[1]; + cs_sync_antenna = data[2]; + + if (local_role == BLE_HS_CS_ROLE_INITIATOR) { + if (data_len < 5) { + /* Ignore invalid formatted results */ + return 1; + } + + measured_freq_offset = get_le16(data + 3); + } + + (void)measured_freq_offset; + (void)packet_quality; + (void)packet_rssi; + (void)cs_sync_antenna; + + return 0; +} + +static int +ble_cs_add_mode1_result(const uint8_t *data, uint8_t data_len) +{ + uint8_t packet_quality; + uint8_t packet_nadm; + uint8_t packet_rssi; + uint8_t cs_sync_antenna; + uint32_t packet_pct1; + uint32_t packet_pct2; + int16_t time_diff; + + if (!IN_RANGE(data_len, 6, 14)) { + /* Ignore invalid formatted results */ + return 1; + } + + packet_quality = data[0]; + packet_nadm = data[1]; + packet_rssi = data[2]; + time_diff = get_le16(data + 3); + cs_sync_antenna = data[5]; + + if (data_len == 14) { + packet_pct1 = get_le32(data + 6); + packet_pct2 = get_le32(data + 10); + } + + (void)packet_quality; + (void)packet_nadm; + (void)packet_rssi; + (void)cs_sync_antenna; + /* TODO: Extract IQ samples */ + (void)packet_pct1; + (void)packet_pct2; + + if (time_diff != TIME_DIFF_NOT_AVAILABLE) { + time_diff_sum += time_diff; + ++time_diff_count; + } + + return 0; +} + +static int +ble_cs_add_mode2_result(const uint8_t *data, uint8_t data_len) +{ + uint32_t tone_pct[N_AP_MAX + 1]; + uint8_t tone_quality_ind[N_AP_MAX + 1]; + uint8_t aci; + uint8_t n_ap; + uint8_t i; + + aci = *(data++); + n_ap = aci_table[aci]; + + if (data_len < 1 + n_ap * 4) { + /* Ignore invalid formatted results */ + return 1; + } + + for (i = 0; i < n_ap + 1; ++i) { + tone_pct[i] = get_le24(data); + data += 3; + } + + for (i = 0; i < n_ap + 1; ++i) { + tone_quality_ind[i] = *(data++); + } + + /* TODO: Extract IQ samples */ + (void)tone_pct; + (void)tone_quality_ind; + + return 0; +} + +static int +ble_cs_add_mode3_result(const uint8_t *data_buf, uint8_t data_len) +{ + const uint8_t *data = data_buf; + uint32_t tone_pct[N_AP_MAX + 1]; + uint8_t tone_quality_ind[N_AP_MAX + 1]; + uint32_t packet_pct1; + uint32_t packet_pct2; + int16_t time_diff; + uint8_t aci; + uint8_t n_ap; + uint8_t packet_quality; + uint8_t packet_nadm; + uint8_t packet_rssi; + uint8_t cs_sync_antenna; + uint8_t i; + + packet_quality = data[0]; + packet_nadm = data[1]; + packet_rssi = data[2]; + time_diff = get_le16(data + 3); + cs_sync_antenna = data[5]; + data += 6; + + if (sounding_pct_estimate && rtt_type != BLE_HS_CS_RTT_AA_ONLY) { + packet_pct1 = get_le32(data); + data += 4; + packet_pct2 = get_le32(data + 10); + data += 4; + } + + aci = *(data++); + n_ap = aci_table[aci]; + + for (i = 0; i < n_ap + 1; ++i) { + tone_pct[i] = get_le24(data); + data += 3; + } + + for (i = 0; i < n_ap + 1; ++i) { + tone_quality_ind[i] = *(data++); + } + + if (data_len < data - data_buf) { + /* Ignore invalid formatted results */ + return 1; + } + + (void)tone_pct; + (void)tone_quality_ind; + (void)packet_pct1; + (void)packet_pct2; + (void)aci; + (void)n_ap; + (void)packet_quality; + (void)packet_nadm; + (void)packet_rssi; + (void)cs_sync_antenna; + + if (time_diff != TIME_DIFF_NOT_AVAILABLE) { + time_diff_sum += time_diff; + ++time_diff_count; + } + + return 0; +} + +static int +ble_cs_add_steps(const struct cs_steps_data *step_data, uint8_t step_count) +{ + int rc = 1; + const void *data; + uint8_t data_len; + uint8_t i; + + for (i = 0; i < step_count; ++i) { + data = step_data->data; + data_len = step_data->data_len; + + if (data_len == 0) { + /* Ignore step with missing results */ + continue; + } + + switch (step_data->mode) { + case BLE_HS_CS_MODE0: + rc = ble_cs_add_mode0_result(data, data_len); + break; + case BLE_HS_CS_MODE1: + rc = ble_cs_add_mode1_result(data, data_len); + break; + case BLE_HS_CS_MODE2: + rc = ble_cs_add_mode2_result(data, data_len); + break; + case BLE_HS_CS_MODE3: + rc = ble_cs_add_mode3_result(data, data_len); + break; + default: + rc = 1; + } + + if (rc) { + /* Ignore invalid formatted results */ + return 0; + } + + step_data += step_data->data_len; + } + return rc; +} + int ble_hs_hci_evt_le_cs_subevent_result(uint8_t subevent, const void *data, unsigned int len) { + int rc; const struct ble_hci_ev_le_subev_cs_subevent_result *ev = data; + uint32_t time_diff_ns; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { return BLE_HS_ECONTROLLER; } + rc = ble_cs_add_steps(ev->steps, ev->num_steps_reported); + + if (!rc && time_diff_count && ev->subevent_done_status != BLE_HS_CS_SUBEVENT_DONE_STATUS_PARTIAL) { + time_diff_ns = time_diff_sum / time_diff_count * 0.5; + time_diff_sum = 0; + time_diff_count = 0; + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), 0, time_diff_ns); + } + return 0; } @@ -664,12 +943,23 @@ int ble_hs_hci_evt_le_cs_subevent_result_continue(uint8_t subevent, const void *data, unsigned int len) { + int rc; const struct ble_hci_ev_le_subev_cs_subevent_result_continue *ev = data; + uint32_t time_diff_ns; - if (len != sizeof(*ev)) { + if (len < sizeof(*ev)) { return BLE_HS_ECONTROLLER; } + rc = ble_cs_add_steps(ev->steps, ev->num_steps_reported); + + if (!rc && time_diff_count && ev->subevent_done_status != BLE_HS_CS_SUBEVENT_DONE_STATUS_PARTIAL) { + time_diff_ns = time_diff_sum / time_diff_count * 0.5; + time_diff_sum = 0; + time_diff_count = 0; + ble_cs_call_procedure_complete_cb(le16toh(ev->conn_handle), 0, time_diff_ns); + } + return 0; } @@ -689,7 +979,6 @@ ble_hs_hci_evt_le_cs_test_end_complete(uint8_t subevent, const void *data, int ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_params *params) { - struct ble_hci_le_cs_rd_loc_supp_cap_rp rsp; struct ble_cs_rd_rem_supp_cap_cp cmd; int rc; @@ -701,6 +990,12 @@ ble_cs_initiator_procedure_start(const struct ble_cs_initiator_procedure_start_p * 5. Start the CS Security Start procedure */ + (void) ble_cs_set_chan_class; + (void) ble_cs_remove_config; + (void) ble_cs_wr_cached_rem_fae; + (void) ble_cs_wr_cached_rem_supp_cap; + (void) ble_cs_rd_loc_supp_cap; + cs_state.cb = params->cb; cs_state.cb_arg = params->cb_arg; diff --git a/nimble/include/nimble/ble.h b/nimble/include/nimble/ble.h index d5e41a37c8..97fb1240b0 100644 --- a/nimble/include/nimble/ble.h +++ b/nimble/include/nimble/ble.h @@ -115,7 +115,7 @@ struct ble_mbuf_hdr_rxinfo #define BLE_MBUF_HDR_F_MIC_FAILURE (0x0020) #define BLE_MBUF_HDR_F_SCAN_RSP_TXD (0x0010) #define BLE_MBUF_HDR_F_SCAN_RSP_RXD (0x0008) -#define BLE_MBUF_HDR_F_RXSTATE_MASK (0x0007) +#define BLE_MBUF_HDR_F_RXSTATE_MASK (0x000f) /* Transmit info. NOTE: no flags defined */ struct ble_mbuf_hdr_txinfo @@ -300,6 +300,9 @@ enum ble_error_codes BLE_ERR_LIMIT_REACHED = 0x43, BLE_ERR_OPERATION_CANCELLED = 0x44, BLE_ERR_PACKET_TOO_LONG = 0x45, + BLE_ERR_TOO_LATE = 0x46, + BLE_ERR_TOO_EARLY = 0x47, + BLE_ERR_INSUFFICIENT_CHAN = 0x48, BLE_ERR_MAX = 0xff }; diff --git a/nimble/include/nimble/hci_common.h b/nimble/include/nimble/hci_common.h index 24843de3c2..9b77321bcf 100644 --- a/nimble/include/nimble/hci_common.h +++ b/nimble/include/nimble/hci_common.h @@ -1178,6 +1178,7 @@ struct ble_hci_le_cs_rd_loc_supp_cap_rp { uint16_t optional_t_fcs_times_supported; uint16_t optional_t_pm_times_supported; uint8_t t_sw_time_supported; + uint8_t optional_tx_snr_capability; } __attribute__((packed)); #define BLE_HCI_OCF_LE_CS_RD_REM_SUPP_CAP (0x008A) @@ -1207,6 +1208,7 @@ struct ble_hci_le_cs_wr_cached_rem_supp_cap_cp { uint16_t optional_t_fcs_times_supported; uint16_t optional_t_pm_times_supported; uint8_t t_sw_time_supported; + uint8_t optional_tx_snr_capability; } __attribute__((packed)); struct ble_hci_le_cs_wr_cached_rem_supp_cap_rp { uint16_t conn_handle; @@ -1261,7 +1263,7 @@ struct ble_hci_le_cs_create_config_cp { uint8_t channel_selection_type; uint8_t ch3c_shape; uint8_t ch3c_jump; - uint8_t companion_signal_enable; + uint8_t reserved; } __attribute__((packed)); #define BLE_HCI_OCF_LE_CS_REMOVE_CONFIG (0x0091) @@ -1322,7 +1324,7 @@ struct ble_hci_le_cs_test_cp { uint8_t t_pm_time; uint8_t t_sw_time; uint8_t tone_antenna_config_selection; - uint8_t companion_signal_enable; + uint8_t reserved; uint16_t drbg_nonce; uint16_t override_config; uint8_t override_parameters_length; @@ -2230,6 +2232,7 @@ struct ble_hci_ev_le_subev_cs_rd_rem_supp_cap_complete { uint16_t optional_t_fcs_times_supported; uint16_t optional_t_pm_times_supported; uint8_t t_sw_time_supported; + uint8_t optional_tx_snr_capability; } __attribute__((packed)); #define BLE_HCI_LE_SUBEV_CS_RD_REM_FAE_COMPLETE (0x2D) @@ -2268,7 +2271,7 @@ struct ble_hci_ev_le_subev_cs_config_complete { uint8_t channel_selection_type; uint8_t ch3c_shape; uint8_t ch3c_jump; - uint8_t companion_signal_enable; + uint8_t reserved; uint8_t t_ip1_time; uint8_t t_ip2_time; uint8_t t_fcs_time;