Bluetooth: controller: Refactor sync_info population

cvinayak · cvinayak · commit 155405e827c5 · 2021-07-11T13:38:54.000+05:30
Based on review comments, refactor out sync_info population
to be performed by the caller of the function that prepares
the extended advertising PDU.

Signed-off-by: Vinayak Kariappa Chettimada &lt;vich@nordicsemi.no&gt;
diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv.c b/subsys/bluetooth/controller/ll_sw/ull_adv.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2020 Nordic Semiconductor ASA
+ * Copyright (c) 2016-2021 Nordic Semiconductor ASA
  * Copyright (c) 2016 Vinayak Kariappa Chettimada
  *
  * SPDX-License-Identifier: Apache-2.0
@@ -10,6 +10,7 @@
 #include <zephyr.h>
 #include <soc.h>
 #include <bluetooth/hci.h>
+#include <sys/byteorder.h>
 
 #include "hal/cpu.h"
 #include "hal/ccm.h"
@@ -1208,14 +1209,24 @@ uint8_t ll_adv_enable(uint8_t enable)
 		if (lll->sync) {
 			sync = HDR_LLL2ULL(lll->sync);
 			if (sync->is_enabled && !sync->is_started) {
+				struct pdu_adv_sync_info *sync_info;
+				uint8_t value[1 + sizeof(sync_info)];
 				uint8_t err;
 
 				err = ull_adv_aux_hdr_set_clear(adv,
 					ULL_ADV_PDU_HDR_FIELD_SYNC_INFO,
-					0, NULL, NULL, &pri_idx);
+					0, value, NULL, &pri_idx);
 				if (err) {
 					return err;
 				}
+
+				/* First byte in the length-value encoded
+				 * parameter is size of sync_info structure,
+				 * followed by pointer to sync_info in the
+				 * PDU.
+				 */
+				memcpy(&sync_info, &value[1], sizeof(sync_info));
+				ull_adv_sync_info_fill(sync, sync_info);
 			} else {
 				/* Do not start periodic advertising */
 				sync = NULL;
diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c b/subsys/bluetooth/controller/ll_sw/ull_adv_aux.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020 Nordic Semiconductor ASA
+ * Copyright (c) 2017-2021 Nordic Semiconductor ASA
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -22,7 +22,6 @@
 #include "pdu.h"
 
 #include "lll.h"
-#include "lll_clock.h"
 #include "lll/lll_vendor.h"
 #include "lll/lll_adv_types.h"
 #include "lll_adv.h"
@@ -47,10 +46,6 @@ static int init_reset(void);
 #if (CONFIG_BT_CTLR_ADV_AUX_SET > 0)
 static inline struct ll_adv_aux_set *aux_acquire(void);
 static inline void aux_release(struct ll_adv_aux_set *aux);
-#if defined(CONFIG_BT_CTLR_ADV_PERIODIC)
-static inline void sync_info_fill(struct lll_adv_sync *lll_sync,
-				  uint8_t **dptr);
-#endif /* CONFIG_BT_CTLR_ADV_PERIODIC */
 static void mfy_aux_offset_get(void *param);
 static void ticker_cb(uint32_t ticks_at_expire, uint32_t remainder,
 		      uint16_t lazy, uint8_t force, void *param);
@@ -441,6 +436,7 @@ uint8_t ull_adv_aux_hdr_set_clear(struct ll_adv_set *adv,
 	struct pdu_adv_ext_hdr *sec_hdr, sec_hdr_prev;
 	struct pdu_adv *pri_pdu, *pri_pdu_prev;
 	struct pdu_adv *sec_pdu_prev, *sec_pdu;
+	struct pdu_adv_sync_info *sync_info;
 	uint8_t *pri_dptr, *pri_dptr_prev;
 	uint8_t *sec_dptr, *sec_dptr_prev;
 	uint8_t pri_len, sec_len_prev;
@@ -611,16 +607,30 @@ uint8_t ull_adv_aux_hdr_set_clear(struct ll_adv_set *adv,
 
 	/* No SyncInfo flag in primary channel PDU */
 	/* Add/Remove SyncInfo flag in secondary channel PDU */
-	if ((sec_hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_SYNC_INFO) ||
-	    (!(sec_hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_SYNC_INFO) &&
-	     sec_hdr_prev.sync_info)) {
+	if (sec_hdr_add_fields & ULL_ADV_PDU_HDR_FIELD_SYNC_INFO) {
 		sec_hdr->sync_info = 1;
-	}
-	if (sec_hdr_prev.sync_info) {
-		sec_dptr_prev += sizeof(struct pdu_adv_sync_info);
-	}
-	if (sec_hdr->sync_info) {
-		sec_dptr += sizeof(struct pdu_adv_sync_info);
+		sync_info = NULL;
+
+		/* return the size of sync info structure */
+		*(uint8_t *)value = sizeof(*sync_info);
+		value = (uint8_t *)value + 1;
+
+		/* return the pointer to sync info struct inside the PDU
+		 * buffer
+		 */
+		memcpy(value, &sec_dptr, sizeof(sec_dptr));
+		value = (uint8_t *)value + sizeof(sec_dptr);
+
+		sec_dptr += sizeof(*sync_info);
+	} else if (!(sec_hdr_rem_fields & ULL_ADV_PDU_HDR_FIELD_SYNC_INFO) &&
+		   sec_hdr_prev.sync_info) {
+		sec_hdr->sync_info = 1;
+		sync_info = (void *)sec_dptr_prev;
+
+		sec_dptr_prev += sizeof(*sync_info);
+		sec_dptr += sizeof(*sync_info);
+	} else {
+		sync_info = NULL;
 	}
 
 	/* Tx Power flag */
@@ -724,10 +734,15 @@ uint8_t ull_adv_aux_hdr_set_clear(struct ll_adv_set *adv,
 	/* No SyncInfo in primary channel PDU */
 	/* Fill SyncInfo in secondary channel PDU */
 	if (sec_hdr_prev.sync_info) {
-		sec_dptr_prev -= sizeof(struct pdu_adv_sync_info);
+		sec_dptr_prev -= sizeof(*sync_info);
 	}
+
 	if (sec_hdr->sync_info) {
-		sync_info_fill(lll->sync, &sec_dptr);
+		sec_dptr -= sizeof(*sync_info);
+	}
+
+	if (sync_info) {
+		memmove(sec_dptr, sync_info, sizeof(*sync_info));
 	}
 #endif /* CONFIG_BT_CTLR_ADV_PERIODIC */
 
@@ -1016,37 +1031,6 @@ inline uint8_t ull_adv_aux_handle_get(struct ll_adv_aux_set *aux)
 			     sizeof(struct ll_adv_aux_set));
 }
 
-#if defined(CONFIG_BT_CTLR_ADV_PERIODIC)
-static inline void sync_info_fill(struct lll_adv_sync *lll_sync,
-				  uint8_t **dptr)
-{
-	struct ll_adv_sync_set *sync;
-	struct pdu_adv_sync_info *si;
-
-	*dptr -= sizeof(*si);
-	si = (void *)*dptr;
-
-	/* NOTE: sync offset and offset unit filled by secondary prepare */
-	si->offs_units = 0U;
-	/* If sync_info is part of ADV PDU the offs_adjust field
-	 * is always set to 0.
-	 */
-	si->offs_adjust = 0U;
-	si->offs = 0U;
-
-	sync = HDR_LLL2ULL(lll_sync);
-	si->interval = sys_cpu_to_le16(sync->interval);
-	memcpy(si->sca_chm, lll_sync->data_chan_map,
-	       sizeof(si->sca_chm));
-	si->sca_chm[4] &= 0x1f;
-	si->sca_chm[4] |= lll_clock_sca_local_get() << 5;
-	memcpy(&si->aa, lll_sync->access_addr, sizeof(si->aa));
-	memcpy(si->crc_init, lll_sync->crc_init, sizeof(si->crc_init));
-
-	si->evt_cntr = 0U; /* NOTE: Filled by secondary prepare */
-}
-#endif /* CONFIG_BT_CTLR_ADV_PERIODIC */
-
 static void mfy_aux_offset_get(void *param)
 {
 	struct ll_adv_set *adv = param;
diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h b/subsys/bluetooth/controller/ll_sw/ull_adv_internal.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020 Nordic Semiconductor ASA
+ * Copyright (c) 2017-2021 Nordic Semiconductor ASA
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -111,39 +111,6 @@ uint8_t ull_adv_aux_hdr_set_clear(struct ll_adv_set *adv,
 				  struct pdu_adv_adi *adi,
 				  uint8_t *pri_idx);
 
-/* helper function to release periodic advertising instance */
-void ull_adv_sync_release(struct ll_adv_sync_set *sync);
-
-/* helper function to allocate new PDU data for AUX_SYNC_IND and return
- * previous and new PDU for further processing.
- */
-uint8_t ull_adv_sync_pdu_alloc(struct ll_adv_set *adv,
-			       uint16_t hdr_add_fields,
-			       uint16_t hdr_rem_fields,
-			       struct ull_adv_ext_hdr_data *hdr_data,
-			       struct pdu_adv **ter_pdu_prev,
-			       struct pdu_adv **ter_pdu_new,
-			       void **extra_data_prev,
-			       void **extra_data_new,
-			       uint8_t *ter_idx);
-
-/* helper function to set/clear common extended header format fields
- * for AUX_SYNC_IND PDU.
- */
-uint8_t ull_adv_sync_pdu_set_clear(struct lll_adv_sync *lll_sync,
-				   struct pdu_adv *ter_pdu_prev,
-				   struct pdu_adv *ter_pdu,
-				   uint16_t hdr_add_fields,
-				   uint16_t hdr_rem_fields,
-				   struct ull_adv_ext_hdr_data *hdr_data);
-
-/* helper function to update extra_data field */
-void ull_adv_sync_extra_data_set_clear(void *extra_data_prev,
-				       void *extra_data_new,
-				       uint16_t hdr_add_fields,
-				       uint16_t hdr_rem_fields,
-				       void *data);
-
 /* helper function to calculate common ext adv payload header length and
  * adjust the data pointer.
  * NOTE: This function reverts the header data pointer if there is no
@@ -184,10 +151,47 @@ uint32_t ull_adv_sync_start(struct ll_adv_set *adv,
 			    struct ll_adv_sync_set *sync,
 			    uint32_t ticks_anchor);
 
+/* helper function to release periodic advertising instance */
+void ull_adv_sync_release(struct ll_adv_sync_set *sync);
+
+/* helper function to fill initial value of sync_info structure */
+void ull_adv_sync_info_fill(struct ll_adv_sync_set *sync,
+			    struct pdu_adv_sync_info *si);
+
 /* helper function to update periodic advertising event length */
 void ull_adv_sync_update(struct ll_adv_sync_set *sync, uint32_t slot_plus_us,
 			 uint32_t slot_minus_us);
 
+/* helper function to allocate new PDU data for AUX_SYNC_IND and return
+ * previous and new PDU for further processing.
+ */
+uint8_t ull_adv_sync_pdu_alloc(struct ll_adv_set *adv,
+			       uint16_t hdr_add_fields,
+			       uint16_t hdr_rem_fields,
+			       struct ull_adv_ext_hdr_data *hdr_data,
+			       struct pdu_adv **ter_pdu_prev,
+			       struct pdu_adv **ter_pdu_new,
+			       void **extra_data_prev,
+			       void **extra_data_new,
+			       uint8_t *ter_idx);
+
+/* helper function to set/clear common extended header format fields
+ * for AUX_SYNC_IND PDU.
+ */
+uint8_t ull_adv_sync_pdu_set_clear(struct lll_adv_sync *lll_sync,
+				   struct pdu_adv *ter_pdu_prev,
+				   struct pdu_adv *ter_pdu,
+				   uint16_t hdr_add_fields,
+				   uint16_t hdr_rem_fields,
+				   struct ull_adv_ext_hdr_data *hdr_data);
+
+/* helper function to update extra_data field */
+void ull_adv_sync_extra_data_set_clear(void *extra_data_prev,
+				       void *extra_data_new,
+				       uint16_t hdr_add_fields,
+				       uint16_t hdr_rem_fields,
+				       void *data);
+
 /* helper function to schedule a mayfly to get sync offset */
 void ull_adv_sync_offset_get(struct ll_adv_set *adv);
 
diff --git a/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c b/subsys/bluetooth/controller/ll_sw/ull_adv_sync.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020 Nordic Semiconductor ASA
+ * Copyright (c) 2017-2021 Nordic Semiconductor ASA
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -22,6 +22,7 @@
 #include "pdu.h"
 
 #include "lll.h"
+#include "lll_clock.h"
 #include "lll/lll_vendor.h"
 #include "lll/lll_adv_types.h"
 #include "lll_adv.h"
@@ -525,6 +526,8 @@ uint8_t ll_adv_sync_enable(uint8_t handle, uint8_t enable)
 	}
 
 	if (adv->is_enabled && !sync->is_started) {
+		struct pdu_adv_sync_info *sync_info;
+		uint8_t value[1 + sizeof(sync_info)];
 		uint32_t ticks_slot_overhead_aux;
 		struct lll_adv_aux *lll_aux;
 		struct ll_adv_aux_set *aux;
@@ -537,11 +540,18 @@ uint8_t ll_adv_sync_enable(uint8_t handle, uint8_t enable)
 		/* Add sync_info into auxiliary PDU */
 		err = ull_adv_aux_hdr_set_clear(adv,
 						ULL_ADV_PDU_HDR_FIELD_SYNC_INFO,
-						0, NULL, NULL, &pri_idx);
+						0, value, NULL, &pri_idx);
 		if (err) {
 			return err;
 		}
 
+		/* First byte in the length-value encoded parameter is size of
+		 * sync_info structure, followed by pointer to sync_info in the
+		 * PDU.
+		 */
+		memcpy(&sync_info, &value[1], sizeof(sync_info));
+		ull_adv_sync_info_fill(sync, sync_info);
+
 		if (lll_aux) {
 			/* FIXME: Find absolute ticks until after auxiliary PDU
 			 *        on air to place the periodic advertising PDU.
@@ -695,6 +705,34 @@ void ull_adv_sync_release(struct ll_adv_sync_set *sync)
 	sync_release(sync);
 }
 
+void ull_adv_sync_info_fill(struct ll_adv_sync_set *sync,
+			    struct pdu_adv_sync_info *si)
+{
+	struct lll_adv_sync *lll_sync;
+
+	/* NOTE: sync offset and offset unit filled by secondary prepare.
+	 *
+	 * If sync_info is part of ADV PDU the offs_adjust field
+	 * is always set to 0.
+	 */
+	si->offs_units = 0U;
+	si->offs_adjust = 0U;
+	si->offs = 0U;
+
+	/* Fill the interval, channel map, access address and CRC init */
+	si->interval = sys_cpu_to_le16(sync->interval);
+	lll_sync = &sync->lll;
+	memcpy(si->sca_chm, lll_sync->data_chan_map,
+	       sizeof(si->sca_chm));
+	si->sca_chm[4] &= 0x1f;
+	si->sca_chm[4] |= lll_clock_sca_local_get() << 5;
+	memcpy(&si->aa, lll_sync->access_addr, sizeof(si->aa));
+	memcpy(si->crc_init, lll_sync->crc_init, sizeof(si->crc_init));
+
+	/* NOTE: Filled by secondary prepare */
+	si->evt_cntr = 0U;
+}
+
 void ull_adv_sync_offset_get(struct ll_adv_set *adv)
 {
 	static memq_link_t link;
