Bluetooth: Controller: Fix pdu_free_sem_give assertion under ZLI use

Fix assertion due to multiple mayfly_enqueue calls used
under ZLI when pdu_free_sem_give is invoked from the LLL.

Signed-off-by: Vinayak Kariappa Chettimada <[email protected]>

Author: cvinayak

subsys/bluetooth/controller/ll_sw/nordic/lll/lll_adv.c

@@ -54,6 +54,8 @@
 
 static int init_reset(void);
 static void pdu_free_sem_give(void);
+static void pdu_free_sem_take_all_no_wait(void);
+static int pdu_free_sem_take_all(void);
 
 #if defined(CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY)
 static inline void adv_extra_data_release(struct lll_adv_pdu *pdu, int idx);
@@ -410,8 +412,7 @@ struct pdu_adv *lll_adv_pdu_alloc_pdu_adv(void)
 
 	p = MFIFO_DEQUEUE_PEEK(pdu_free);
 	if (p) {
-		err = k_sem_take(&sem_pdu_free, K_NO_WAIT);
-		LL_ASSERT(!err);
+		pdu_free_sem_take_all_no_wait();
 
 		MFIFO_DEQUEUE(pdu_free);
 
@@ -429,7 +430,7 @@ struct pdu_adv *lll_adv_pdu_alloc_pdu_adv(void)
 		return p;
 	}
 
-	err = k_sem_take(&sem_pdu_free, K_FOREVER);
+	err = pdu_free_sem_take_all();
 	LL_ASSERT(!err);
 
 	p = MFIFO_DEQUEUE(pdu_free);
@@ -803,11 +804,10 @@ static void pdu_free_sem_give(void)
 {
 	static memq_link_t link;
 	static struct mayfly mfy = {0, 0, &link, NULL, mfy_pdu_free_sem_give};
-	uint32_t retval;
 
-	retval = mayfly_enqueue(TICKER_USER_ID_LLL, TICKER_USER_ID_ULL_HIGH, 0,
-				&mfy);
-	LL_ASSERT(!retval);
+	/* Ignore mayfly_enqueue failure on repeated enqueue call */
+	(void)mayfly_enqueue(TICKER_USER_ID_LLL, TICKER_USER_ID_ULL_HIGH, 0,
+			     &mfy);
 }
 
 #else /* !CONFIG_BT_CTLR_ZLI */
@@ -817,6 +817,33 @@ static void pdu_free_sem_give(void)
 }
 #endif /* !CONFIG_BT_CTLR_ZLI */
 
+static void pdu_free_sem_take_all_no_wait(void)
+{
+	int err;
+
+	/* Flush all given semaphore count due to multiple release from LLL
+	 * context.
+	 */
+	do {
+		err = k_sem_take(&sem_pdu_free, K_NO_WAIT);
+	} while (!err);
+}
+
+static int pdu_free_sem_take_all(void)
+{
+	int err;
+
+	/* Wait for first sem give */
+	err = k_sem_take(&sem_pdu_free, K_FOREVER);
+	if (err) {
+		return err;
+	}
+
+	/* Flush all subsequent sem give */
+	pdu_free_sem_take_all_no_wait();
+
+	return 0;
+}
 #if defined(CONFIG_BT_CTLR_ADV_EXT_PDU_EXTRA_DATA_MEMORY)
 static void *adv_extra_data_allocate(struct lll_adv_pdu *pdu, uint8_t last)
 {