Bluetooth: controller: Add Periodic Sync drift compensation

Added implementation to handle Periodic Advertising clock drift in the created Periodic Advertising Sync instance. Signed-off-by: Vinayak Kariappa Chettimada <[email protected]>
k0d · Oct 7, 2020 · d1f4611 · d1f4611
1 parent 31f2196
commit d1f4611
Show file tree

Hide file tree

Showing 6 changed files with 91 additions and 27 deletions.
diff --git a/subsys/bluetooth/controller/ll_sw/lll.h b/subsys/bluetooth/controller/ll_sw/lll.h
@@ -298,6 +298,9 @@ enum {
 #if defined(CONFIG_BT_OBSERVER)
 #if defined(CONFIG_BT_CTLR_ADV_EXT)
 	EVENT_DONE_EXTRA_TYPE_SCAN_AUX,
+#if defined(CONFIG_BT_CTLR_SCAN_PERIODIC)
+	EVENT_DONE_EXTRA_TYPE_SYNC,
+#endif /* CONFIG_BT_CTLR_SCAN_PERIODIC */
 #endif /* CONFIG_BT_CTLR_ADV_EXT */
 #endif /* CONFIG_BT_OBSERVER */
 

diff --git a/subsys/bluetooth/controller/ll_sw/lll_sync.h b/subsys/bluetooth/controller/ll_sw/lll_sync.h
@@ -22,7 +22,6 @@ struct lll_sync {
 	uint32_t window_widening_max_us;
 	uint32_t window_widening_prepare_us;
 	uint32_t window_widening_event_us;
-	uint32_t window_size_prepare_us;
 	uint32_t window_size_event_us;
 
 	uint8_t phy:3;

diff --git a/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_sync.c b/subsys/bluetooth/controller/ll_sw/nordic/lll/lll_sync.c
@@ -134,10 +134,6 @@ static int prepare_cb(struct lll_prepare_param *p)
 		lll->window_widening_event_us =	lll->window_widening_max_us;
 	}
 
-	/* Current window size */
-	lll->window_size_event_us += lll->window_size_prepare_us;
-	lll->window_size_prepare_us = 0;
-
 	/* Calculate the radio channel to use */
 	data_chan_use = lll_chan_sel_2(event_counter, lll->data_chan_id,
 				       &lll->data_chan_map[0],
@@ -185,7 +181,6 @@ static int prepare_cb(struct lll_prepare_param *p)
 	remainder_us = radio_tmr_start(0, ticks_at_start, remainder);
 
 	radio_tmr_aa_capture();
-	radio_tmr_aa_save(0);
 
 	hcto = remainder_us +
 	       ((EVENT_JITTER_US + EVENT_TICKER_RES_MARGIN_US +
@@ -238,22 +233,17 @@ static int prepare_cb(struct lll_prepare_param *p)
 	}
 
 	DEBUG_RADIO_START_O(1);
-
-	printk("SYNC PREPARE\n");
 	return 0;
 }
 
 static void isr_rx(void *param)
 {
+	struct lll_sync *lll = param;
+	struct event_done_extra *e;
 	uint8_t rssi_ready;
 	uint8_t trx_done;
 	uint8_t crc_ok;
 
-	/* TODO: may be we dont need to profile, as there is no use of tIFS */
-	if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) {
-		lll_prof_latency_capture();
-	}
-
 	/* Read radio status and events */
 	trx_done = radio_is_done();
 	if (trx_done) {
@@ -270,9 +260,7 @@ static void isr_rx(void *param)
 	if (!trx_done) {
 		/* TODO: Combine the early exit with above if-then-else block
 		 */
-		printk("RX FAILED\n");
-
-		return;
+		goto isr_rx_exit;
 	}
 
 	if (crc_ok) {
@@ -282,15 +270,28 @@ static void isr_rx(void *param)
 		printk("CRC BAD\n");
 	}
 
-	if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) {
-		lll_prof_cputime_capture();
-	}
+	/* Calculate and place the drift information in done event */
+	e = ull_event_done_extra_get();
+	LL_ASSERT(e);
 
-	/* TODO: drift compensation */
+	e->type = EVENT_DONE_EXTRA_TYPE_SYNC;
+	e->trx_cnt = 1U;
+	e->crc_valid = crc_ok;
 
-	lll_isr_cleanup(param);
+#if defined(CONFIG_BT_CTLR_PHY)
+	e->drift.preamble_to_addr_us = addr_us_get(lll->phy);
+#else /* !CONFIG_BT_CTLR_PHY */
+	e->drift.preamble_to_addr_us = addr_us_get(0);
+#endif /* !CONFIG_BT_CTLR_PHY */
 
-	if (IS_ENABLED(CONFIG_BT_CTLR_PROFILE_ISR)) {
-		lll_prof_send();
-	}
+	e->drift.start_to_address_actual_us = radio_tmr_aa_get() -
+					      radio_tmr_ready_get();
+	e->drift.window_widening_event_us = lll->window_widening_event_us;
+
+	/* Reset window widening, as anchor point sync-ed */
+	lll->window_widening_event_us = 0U;
+	lll->window_size_event_us = 0U;
+
+isr_rx_exit:
+	lll_isr_cleanup(param);
 }
diff --git a/subsys/bluetooth/controller/ll_sw/ull.c b/subsys/bluetooth/controller/ll_sw/ull.c
@@ -32,6 +32,7 @@
 #include "ll_feat.h"
 #include "ll_settings.h"
 #include "lll.h"
+#include "lll_vendor.h"
 #include "lll_adv.h"
 #include "lll_scan.h"
 #include "lll_sync.h"
@@ -1885,6 +1886,11 @@ static inline void rx_demux_event_done(memq_link_t *link,
 	case EVENT_DONE_EXTRA_TYPE_SCAN_AUX:
 		ull_scan_aux_done(done);
 		break;
+#if defined(CONFIG_BT_CTLR_SCAN_PERIODIC)
+	case EVENT_DONE_EXTRA_TYPE_SYNC:
+		ull_sync_done(done);
+		break;
+#endif /* CONFIG_BT_CTLR_SCAN_PERIODIC */
 #endif /* CONFIG_BT_OBSERVER */
 #endif /* CONFIG_BT_CTLR_ADV_EXT */
 

diff --git a/subsys/bluetooth/controller/ll_sw/ull_sync.c b/subsys/bluetooth/controller/ll_sw/ull_sync.c
@@ -46,6 +46,7 @@ static struct ll_sync_set *is_enabled_get(uint16_t handle);
 static void ticker_cb(uint32_t ticks_at_expire, uint32_t remainder,
 		      uint16_t lazy, void *param);
 static void ticker_op_cb(uint32_t status, void *param);
+static void ticker_update_sync_op_cb(uint32_t status, void *param);
 
 static struct ll_sync_set ll_sync_pool[CONFIG_BT_CTLR_SCAN_SYNC_SET];
 static void *sync_free;
@@ -58,6 +59,7 @@ uint8_t ll_sync_create(uint8_t options, uint8_t sid, uint8_t adv_addr_type,
 	memq_link_t *link_sync_estab;
 	memq_link_t *link_sync_lost;
 	struct node_rx_hdr *node_rx;
+	struct lll_sync *lll_sync;
 	struct ll_scan_set *scan;
 	struct ll_sync_set *sync;
 
@@ -117,8 +119,16 @@ uint8_t ll_sync_create(uint8_t options, uint8_t sid, uint8_t adv_addr_type,
 
 	/* TODO: Support for CTE type */
 
+	/* Initialize sync context */
+	lll_sync = &sync->lll;
+	lll_sync->latency_prepare = 0U;
+	lll_sync->latency_event = 0U;
+	lll_sync->data_chan_id = 0U;
+	lll_sync->window_widening_prepare_us = 0U;
+	lll_sync->window_widening_event_us = 0U;
+
 	/* Reporting initially enabled/disabled */
-	sync->lll.is_enabled = options & BIT(1);
+	lll_sync->is_enabled = options & BIT(1);
 
 	/* Initialise state */
 	scan->per_scan.state = LL_SYNC_STATE_IDLE;
@@ -130,7 +140,7 @@ uint8_t ll_sync_create(uint8_t options, uint8_t sid, uint8_t adv_addr_type,
 
 	/* Initialise ULL and LLL headers */
 	ull_hdr_init(&sync->ull);
-	lll_hdr_init(&sync->lll, sync);
+	lll_hdr_init(lll_sync, sync);
 
 	/* Enable scanner to create sync */
 	scan->per_scan.sync = sync;
@@ -346,7 +356,7 @@ void ull_sync_setup(struct ll_scan_set *scan, struct ll_scan_aux_set *aux,
 	interval_us -= lll->window_widening_periodic_us;
 
 	/* TODO: active_to_start feature port */
-	sync->evt.ticks_active_to_start = 0;
+	sync->evt.ticks_active_to_start = 0U;
 	sync->evt.ticks_xtal_to_start =
 		HAL_TICKER_US_TO_TICKS(EVENT_OVERHEAD_XTAL_US);
 	sync->evt.ticks_preempt_to_start =
@@ -380,6 +390,43 @@ void ull_sync_setup(struct ll_scan_set *scan, struct ll_scan_aux_set *aux,
 		  (ret == TICKER_STATUS_BUSY));
 }
 
+void ull_sync_done(struct node_rx_event_done *done)
+{
+	struct lll_sync *lll = (void *)HDR_ULL2LLL(done->param);
+	uint32_t ticks_drift_minus;
+	uint32_t ticks_drift_plus;
+	uint16_t lazy = 0U;
+	uint8_t force = 0U;
+
+	/* TODO: use skip value and decide on the laziness using latency_event.
+	 */
+
+	ull_drift_ticks_get(done, &ticks_drift_plus, &ticks_drift_minus);
+	if (ticks_drift_plus || ticks_drift_minus || lazy || force) {
+		struct ll_sync_set *sync = (void *)HDR_LLL2EVT(lll);
+		uint16_t sync_handle = ull_sync_handle_get(sync);
+		uint32_t ticker_status;
+
+		/* Call to ticker_update can fail under the race
+		 * condition where in the periodic sync role is being stopped
+		 * but at the same time it is preempted by periodic sync event
+		 * that gets into close state. Accept failure when periodic sync
+		 * role is being stopped.
+		 */
+		ticker_status = ticker_update(TICKER_INSTANCE_ID_CTLR,
+					      TICKER_USER_ID_ULL_HIGH,
+					      (TICKER_ID_SCAN_SYNC_BASE +
+					       sync_handle),
+					      ticks_drift_plus,
+					      ticks_drift_minus, 0, 0,
+					      lazy, force,
+					      ticker_update_sync_op_cb, sync);
+		LL_ASSERT((ticker_status == TICKER_STATUS_SUCCESS) ||
+			  (ticker_status == TICKER_STATUS_BUSY) ||
+			  ((void *)sync == ull_disable_mark_get()));
+	}
+}
+
 static int init_reset(void)
 {
 	/* Initialize sync pool. */
@@ -447,3 +494,10 @@ static void ticker_op_cb(uint32_t status, void *param)
 
 	LL_ASSERT(status == TICKER_STATUS_SUCCESS);
 }
+
+static void ticker_update_sync_op_cb(uint32_t status, void *param)
+{
+	LL_ASSERT(status == TICKER_STATUS_SUCCESS ||
+		  param == ull_update_mark_get() ||
+		  param == ull_disable_mark_get());
+}
diff --git a/subsys/bluetooth/controller/ll_sw/ull_sync_internal.h b/subsys/bluetooth/controller/ll_sw/ull_sync_internal.h
@@ -10,3 +10,4 @@ uint16_t ull_sync_handle_get(struct ll_sync_set *sync);
 void ull_sync_release(struct ll_sync_set *sync);
 void ull_sync_setup(struct ll_scan_set *scan, struct ll_scan_aux_set *aux,
 		    struct node_rx_hdr *node_rx, struct pdu_adv_sync_info *si);
+void ull_sync_done(struct node_rx_event_done *done);