nimble/ll: Add CS_SYNC transmission/reception

- CS SYNC sequence generation
- CS Access Address matching

nimble/controller/src/ble_ll_cs.c

@@ -31,7 +31,7 @@
 #include "ble_ll_cs_priv.h"
 
 struct ble_ll_cs_supp_cap g_ble_ll_cs_local_cap;
-static struct ble_ll_cs_sm g_ble_ll_cs_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)];
+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};

nimble/controller/src/ble_ll_cs_sync.c

@@ -21,11 +21,67 @@
 #if MYNEWT_VAL(BLE_LL_CHANNEL_SOUNDING)
 #include <stdint.h>
 #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_priv.h"
 #include "ble_ll_cs_priv.h"
 
+extern struct ble_ll_cs_sm g_ble_ll_cs_sm[MYNEWT_VAL(BLE_MAX_CONNECTIONS)];
 extern struct ble_ll_cs_sm *g_ble_ll_cs_sm_current;
+extern int8_t g_ble_ll_tx_power;
+
+static uint8_t
+ble_ll_cs_sync_make(struct ble_ll_cs_sm *cssm, uint8_t *buf)
+{
+ int i;
+ int rc;
+ uint32_t aa;
+ struct ble_ll_cs_config *conf = cssm->active_config;
+ uint8_t sequence_len;
+ uint8_t bits;
+
+ if (conf->rtt_type != BLE_LL_CS_RTT_AA_ONLY) {
+ rc = ble_ll_cs_drbg_generate_sync_sequence(
+ &cssm->drbg_ctx, cssm->steps_in_procedure_count, conf->rtt_type,
+ buf, &sequence_len);
+
+ if (rc) {
+ return 0;
+ }
+ }
+
+ /* Shift by 4 bits to make space for trailer bits */
+ if (sequence_len > 0) {
+ assert(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 (conf->role == BLE_LL_CS_ROLE_INITIATOR) {
+ aa = cssm->initiator_aa;
+ } else {
+ aa = cssm->reflector_aa;
+ }
+
+ /* Set the trailer bits */
+ if (aa & 0x80000000) {
+ buf[0] |= 0x0A;
+ } else {
+ buf[0] |= 0x05;
+ }
+ ++sequence_len;
+
+ return sequence_len;
+}
 
 /**
  * Called when a CS pkt has been transmitted.
@@ -43,22 +99,116 @@ ble_ll_cs_sync_tx_end_cb(void *arg)
  ble_ll_cs_proc_schedule_next_tx_or_rx(cssm);
 }
 
+static uint8_t
+ble_ll_cs_sync_tx_make(uint8_t *dptr, void *arg, uint8_t *hdr_byte)
+{
+ uint8_t pdu_len;
+ struct ble_ll_cs_sm *cssm = arg;
+
+ /* TODO: Unused fields in CS Sync packet */
+ pdu_len = ble_ll_cs_sync_make(cssm, dptr);
+ *hdr_byte = 0;
+
+ return pdu_len;
+}
+
 int
 ble_ll_cs_sync_tx_sched_cb(struct ble_ll_sched_item *sch)
 {
- /* TODO: Start TX of CS SYNC */
+ int rc;
+ struct ble_ll_cs_sm *cssm = sch->cb_arg;
+
+ BLE_LL_ASSERT(ble_ll_state_get() == BLE_LL_STATE_STANDBY);
+
+ /* TODO: Add to PHY a support CS mode.
+ * Adjustments for nrf:
+ * - Turn off CRC
+ * - set 4 bits in S1
+ *
+ * ble_ll_phy_to_cs_sync_mode(cssm->active_config->cs_sync_phy, 0);
+ */
+
+ ble_ll_tx_power_set(g_ble_ll_tx_power);
+
+ rc = ble_phy_tx_set_start_time(sch->start_time + g_ble_ll_sched_offset_ticks,
+ sch->remainder);
+ if (rc) {
+ return BLE_LL_SCHED_STATE_DONE;
+ }
+
+ ble_phy_set_txend_cb(ble_ll_cs_sync_tx_end_cb, cssm);
+
+ /* TODO: Add to PHY a support for 72 RF channels for CS exchanges.
+ * ble_phy_cs_setchan(cssm->channel, cssm->tx_aa, 0);
+ */
+
+ /* TODO: Add to PHY a support for CS_SYNC transmission.
+ * rc = ble_phy_tx_cs_sync(ble_ll_cs_sync_tx_make, cssm,
+ * BLE_PHY_TRANSITION_TX_RX);
+ */
+ (void) ble_ll_cs_sync_tx_make;
+ if (rc) {
+ ble_phy_disable();
+ return BLE_LL_SCHED_STATE_DONE;
+ }
+
+ ble_ll_state_set(BLE_LL_STATE_CS);
 
  return BLE_LL_SCHED_STATE_RUNNING;
 }
 
 int
 ble_ll_cs_sync_rx_sched_cb(struct ble_ll_sched_item *sch)
 {
- /* TODO: Start RX of CS SYNC */
+ int rc;
+ uint32_t wfr_usecs;
+ struct ble_ll_cs_sm *cssm = sch->cb_arg;
+
+ BLE_LL_ASSERT(ble_ll_state_get() == BLE_LL_STATE_STANDBY);
+
+ /* TODO: Add to PHY a support CS mode.
+ * Adjustments for nrf:
+ * - Turn off CRC
+ * - set 4 bits in S1
+ *
+ * ble_ll_phy_to_cs_sync_mode(cssm->active_config->cs_sync_phy, 0);
+ */
+
+ /* TODO: Add to PHY a support for 72 RF channels for CS exchanges.
+ * ble_phy_cs_setchan(cssm->channel, cssm->tx_aa, 0);
+ */
+
+ rc = ble_phy_rx_set_start_time(sch->start_time + g_ble_ll_sched_offset_ticks,
+ sch->remainder);
+ if (rc) {
+ return BLE_LL_SCHED_STATE_DONE;
+ }
+
+ wfr_usecs = cssm->duration_usecs;
+ ble_phy_wfr_enable(BLE_PHY_WFR_ENABLE_RX, 0, wfr_usecs);
+
+ ble_ll_state_set(BLE_LL_STATE_CS);
 
  return BLE_LL_SCHED_STATE_RUNNING;
 }
 
+static struct ble_ll_cs_sm *
+ble_ll_cs_sync_find_sm_match_aa(uint32_t aa)
+{
+ int i;
+ struct ble_ll_cs_sm *cssm;
+
+ for (i = 0; i < ARRAY_SIZE(g_ble_ll_cs_sm); ++i) {
+ cssm = &g_ble_ll_cs_sm[i];
+
+ if (cssm->rx_aa == aa) {
+ return cssm;
+ }
+ }
+
+ return NULL;
+}
+
 /**
  * Called when a receive PDU has started.
  * Check if the frame is the next expected CS_SYNC packet.
@@ -72,7 +222,14 @@ ble_ll_cs_sync_rx_sched_cb(struct ble_ll_sched_item *sch)
 int
 ble_ll_cs_sync_rx_isr_start(struct ble_mbuf_hdr *rxhdr, uint32_t aa)
 {
- /* TODO: Verify CS Access Address */
+ struct ble_ll_cs_sm *cssm;
+
+ cssm = ble_ll_cs_sync_find_sm_match_aa(aa);
+
+ if (cssm == NULL) {
+ /* This is not the expected packet. Skip the frame. */
+ return -1;
+ }
 
  return 0;
 }
@@ -96,16 +253,16 @@ ble_ll_cs_sync_rx_isr_end(uint8_t *rxbuf, struct ble_mbuf_hdr *rxhdr)
  struct os_mbuf *rxpdu;
  struct ble_ll_cs_sm *cssm = g_ble_ll_cs_sm_current;
 
- assert(cssm != NULL);
- ble_ll_cs_proc_set_now_as_anchor_point(cssm);
- ble_ll_cs_proc_schedule_next_tx_or_rx(cssm);
-
  /* Packet type was verified in isr_start */
 
  rxpdu = ble_ll_rxpdu_alloc(rxbuf[1] + BLE_LL_PDU_HDR_LEN);
  if (rxpdu) {
  ble_phy_rxpdu_copy(rxbuf, rxpdu);
 
+ assert(cssm != NULL);
+ ble_ll_cs_proc_set_now_as_anchor_point(cssm);
+ ble_ll_cs_proc_schedule_next_tx_or_rx(cssm);
+
  /* Send the packet to Link Layer context */
  ble_ll_rx_pdu_in(rxpdu);
  }