Bluetooth: controller: ULL: Add sending of number of CTEs in per adv chain

Direction finding functionality allows to send a number of periodic
advertising PDUs in a chain that have CTE.
ll_df_set_cl_cte_tx_enable function was changed to update periodic
advertising chain to include cte_info field. If the chain is too short,
there is less PDUs in the chain than requested number of CTEs,
the function will add new empty PDUs to the chain end.

Signed-off-by: Piotr Pryga <[email protected]>

Author: ppryga-nordic

subsys/bluetooth/controller/ll_sw/ull_df.c

@@ -98,6 +98,9 @@ static int init_reset(void);
  * @return Pointer to lll_df_adv_cfg or NULL if there is no more free memory.
  */
 static struct lll_df_adv_cfg *df_adv_cfg_acquire(void);
+
+static uint8_t cte_info_set(struct ll_adv_set *adv, struct lll_df_adv_cfg *df_cfg, uint8_t *ter_idx,
+			    struct pdu_adv **first_pdu);
 #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */
 
 /* @brief Function performs ULL Direction Finding initialization
@@ -319,31 +322,11 @@ uint8_t ll_df_set_cl_cte_tx_enable(uint8_t adv_handle, uint8_t cte_enable)
 
 		df_cfg->is_enabled = 0U;
 	} else {
-		struct pdu_cte_info cte_info;
-		void *hdr_data;
-
 		if (df_cfg->is_enabled) {
 			return BT_HCI_ERR_CMD_DISALLOWED;
 		}
 
-		cte_info.type = df_cfg->cte_type;
-		cte_info.time = df_cfg->cte_length;
-		hdr_data = (uint8_t *)&cte_info;
-
-		err = ull_adv_sync_pdu_alloc(adv, ULL_ADV_PDU_EXTRA_DATA_ALLOC_ALWAYS, &pdu_prev,
-					     &pdu, &extra_data_prev, &extra_data, &ter_idx);
-		if (err) {
-			return err;
-		}
-
-		if (extra_data) {
-			ull_adv_sync_extra_data_set_clear(extra_data_prev, extra_data,
-							  ULL_ADV_PDU_HDR_FIELD_CTE_INFO, 0,
-							  df_cfg);
-		}
-
-		err = ull_adv_sync_pdu_set_clear(lll_sync, pdu_prev, pdu,
-						 ULL_ADV_PDU_HDR_FIELD_CTE_INFO, 0, hdr_data);
+		err = cte_info_set(adv, df_cfg, &ter_idx, &pdu);
 		if (err) {
 			return err;
 		}
@@ -665,4 +648,182 @@ static struct lll_df_adv_cfg *df_adv_cfg_acquire(void)
 
 	return df_adv_cfg;
 }
+
+#if (CONFIG_BT_CTLR_DF_PER_ADV_CTE_NUM_MAX > 1)
+/*
+ * @brief Function sets content of cte_info field in periodic advertising chain.
+ *
+ * The function allocates new PDU (or chain of PDUs) for periodic advertising to
+ * fill it with information about CTE, that is going to be transmitted with the PDU.
+ * If there is already allocated PDU, it will be updated to hold information about
+ * CTE.
+ *
+ * @param lll_sync       Pointer to periodic advertising sync object.
+ * @param pdu_prev       Pointer to a PDU that is already in use by LLL or was updated with new PDU
+ *                       payload.
+ * @param pdu            Pointer to a new head of periodic advertising chain. The pointer may have
+ *                       the same value as @p pdu_prev, if payload of PDU pointerd by @p pdu_prev
+ *                       was already updated.
+ * @param cte_count      Number of CTEs that should be transmitted in periodic advertising chain.
+ * @param cte_into       Pointer to instence of cte_info stuctuct that is added to PDUs extended
+ *                       advertising header.
+ *
+ * @return Zero in case of success, other value in case of failure.
+ */
+static uint8_t per_adv_chain_cte_info_set(struct lll_adv_sync *lll_sync, struct pdu_adv *pdu_prev,
+					  struct pdu_adv *pdu, uint8_t cte_count,
+					  struct pdu_cte_info *cte_info)
+{
+	uint8_t pdu_add_field_flags;
+	struct pdu_adv *pdu_next;
+	uint8_t cte_index = 1;
+	bool new_chain;
+	uint8_t err;
+
+	new_chain = (pdu_prev == pdu ? false : true);
+
+	pdu_add_field_flags = ULL_ADV_PDU_HDR_FIELD_CTE_INFO;
+
+	pdu_prev = lll_adv_pdu_linked_next_get(pdu_prev);
+
+	/* Update PDUs in existing chain. Add cte_info to extended advertising header. */
+	while (pdu_prev) {
+		if (new_chain) {
+			pdu_next = lll_adv_pdu_alloc_pdu_adv();
+			lll_adv_pdu_linked_append(pdu_next, pdu);
+			pdu = pdu_next;
+		} else {
+			pdu = lll_adv_pdu_linked_next_get(pdu);
+		}
+
+		pdu_next = lll_adv_pdu_linked_next_get(pdu_prev);
+		/* If all CTEs were added to chain, remove CTE from flags */
+		if (cte_index >= cte_count) {
+			pdu_add_field_flags = 0;
+		} else {
+			++cte_index;
+			/* If it is last PDU in existing chain and there are CTE to be included
+			 * add aux_ptr to flags.
+			 */
+			if (!pdu_next && cte_index < cte_count) {
+				pdu_add_field_flags |= ULL_ADV_PDU_HDR_FIELD_AUX_PTR;
+			}
+		}
+
+		err = ull_adv_sync_pdu_set_clear(lll_sync, pdu_prev, pdu, pdu_add_field_flags, 0,
+						 cte_info);
+		if (err != BT_HCI_ERR_SUCCESS) {
+			/* TODO: implement gracefull error handling, cleanup of
+			 * changed PDUs and notify host abut issue during start of CTE
+			 * transmission.
+			 */
+			return err;
+		}
+		pdu_prev = pdu_next;
+	}
+
+	/* If there is missing only one CTE do not add aux_ptr to PDU */
+	if (cte_count - cte_index >= 2) {
+		pdu_add_field_flags |= ULL_ADV_PDU_HDR_FIELD_AUX_PTR;
+	} else {
+		pdu_add_field_flags = ULL_ADV_PDU_HDR_FIELD_CTE_INFO;
+	}
+
+	/* Add new PDUs if the number of PDUs in existing chain is lower than requested number
+	 * of CTEs.
+	 */
+	while (cte_index < cte_count) {
+		pdu_prev = pdu;
+		pdu = lll_adv_pdu_alloc_pdu_adv();
+		if (!pdu) {
+			/* TODO: implement graceful error handling, cleanup of
+			 * changed PDUs.
+			 */
+			return BT_HCI_ERR_MEM_CAPACITY_EXCEEDED;
+		}
+		ull_adv_sync_pdu_init(pdu, pdu_add_field_flags);
+		ull_adv_sync_pdu_cte_info_set(pdu, cte_info);
+
+		/* Link PDU into a chain */
+		lll_adv_pdu_linked_append(pdu, pdu_prev);
+
+		++cte_index;
+		/* If next PDU in a chain is last PDU, then remove aux_ptr field flag from
+		 * extended advertising header.
+		 */
+		if (cte_index == cte_count - 1) {
+			pdu_add_field_flags &= (~ULL_ADV_PDU_HDR_FIELD_AUX_PTR);
+		}
+	}
+
+	return BT_HCI_ERR_SUCCESS;
+}
+#endif /* CONFIG_BT_CTLR_DF_PER_ADV_CTE_NUM_MAX > 1 */
+
+/*
+ * @brief Function sets content of cte_info field for periodic advertising
+ *
+ * @param adv            Pointer to periodic advertising set.
+ * @param df_cfg         Pointer to direction finding configuration
+ * @param[out] ter_idx   Pointer used to return index of allocated or updated PDU.
+ *                       Index is required for scheduling the PDU for transmission in LLL.
+ * @param[out] first_pdu Pointer to return address of first PDU in a periodic advertising chain
+ *
+ * @return Zero in case of success, other value in case of failure.
+ */
+static uint8_t cte_info_set(struct ll_adv_set *adv, struct lll_df_adv_cfg *df_cfg, uint8_t *ter_idx,
+			    struct pdu_adv **first_pdu)
+{
+	struct pdu_adv *pdu_prev, *pdu;
+	struct lll_adv_sync *lll_sync;
+	struct pdu_cte_info cte_info;
+	uint8_t pdu_add_field_flags;
+	void *extra_data;
+	uint8_t err;
+
+	lll_sync = adv->lll.sync;
+
+	cte_info.type = df_cfg->cte_type;
+	cte_info.time = df_cfg->cte_length;
+
+	/* Note: ULL_ADV_PDU_HDR_FIELD_CTE_INFO is just information that extra_data
+	 * is required in case of this ull_adv_sync_pdu_alloc.
+	 * Other flags here do not change anything. It may be changed to use
+	 * true/false flag for extra_data allocation.
+	 */
+	err = ull_adv_sync_pdu_alloc(adv, ULL_ADV_PDU_EXTRA_DATA_ALLOC_ALWAYS, &pdu_prev, &pdu,
+				     NULL, &extra_data, ter_idx);
+	if (err != BT_HCI_ERR_SUCCESS) {
+		return err;
+	}
+
+	ull_adv_sync_extra_data_set_clear(NULL, extra_data, ULL_ADV_PDU_HDR_FIELD_CTE_INFO, 0,
+					  df_cfg);
+
+#if (CONFIG_BT_CTLR_DF_PER_ADV_CTE_NUM_MAX > 1)
+	if (df_cfg->cte_count > 1) {
+		pdu_add_field_flags =
+			ULL_ADV_PDU_HDR_FIELD_CTE_INFO | ULL_ADV_PDU_HDR_FIELD_AUX_PTR;
+	} else
+#endif /* CONFIG_BT_CTLR_DF_PER_ADV_CTE_NUM_MAX > 1 */
+	{
+		pdu_add_field_flags = ULL_ADV_PDU_HDR_FIELD_CTE_INFO;
+	}
+
+	err = ull_adv_sync_pdu_set_clear(lll_sync, pdu_prev, pdu, pdu_add_field_flags, 0,
+					 &cte_info);
+	if (err != BT_HCI_ERR_SUCCESS) {
+		return err;
+	}
+
+	*first_pdu = pdu;
+#if (CONFIG_BT_CTLR_DF_PER_ADV_CTE_NUM_MAX > 1)
+	err = per_adv_chain_cte_info_set(lll_sync, pdu_prev, pdu, df_cfg->cte_count, &cte_info);
+	if (err != BT_HCI_ERR_SUCCESS) {
+		return err;
+	}
+#endif /* CONFIG_BT_CTLR_DF_PER_ADV_CTE_NUM_MAX > 1 */
+
+	return BT_HCI_ERR_SUCCESS;
+}
 #endif /* CONFIG_BT_CTLR_DF_ADV_CTE_TX */