Bluetooth: Controller: df: Add disable support for CTE REQ and RSP proc

Add missing implementation for disabling CTE request and resposne
control procedures.
If any of these commands is active in LLL then ULL context has
to wait before return. The wait mechanism is based on semaphore.
The semaphore is initialized in ULL context and given by code
responsible for command completion.

Signed-off-by: Piotr Pryga <piotr.pryga@nordicsemi.no>

subsys/bluetooth/controller/ll_sw/ull_conn_types.h

@@ -420,22 +420,29 @@ struct llcp_struct {
 		/* Procedure may be active periodically, active state must be stored.
 		 * If procedure is active, request parameters update may not be issued.
 		 */
-		uint8_t is_enabled;
+		uint8_t is_enabled:1;
+		uint8_t is_active:1;
 		uint8_t cte_type;
 		/* Minimum requested CTE length in 8us units */
 		uint8_t min_cte_len;
 		uint16_t req_interval;
 		uint16_t req_expire;
+		void *disable_param;
+		void (*disable_cb)(void *param);
 	} cte_req;
-
 #endif /* CONFIG_BT_CTLR_DF_CONN_CTE_REQ */
+
 #if defined(CONFIG_BT_CTLR_DF_CONN_CTE_RSP)
 	struct llcp_df_rsp_cfg {
-		uint8_t is_enabled;
+		uint8_t is_enabled:1;
+		uint8_t is_active:1;
 		uint8_t cte_types;
 		uint8_t max_cte_len;
+		void *disable_param;
+		void (*disable_cb)(void *param);
 	} cte_rsp;
 #endif /* CONFIG_BT_CTLR_DF_CONN_CTE_RSP */
+
 #if (CONFIG_BT_CTLR_LLCP_PER_CONN_TX_CTRL_BUF_NUM > 0) &&\
 	(CONFIG_BT_CTLR_LLCP_PER_CONN_TX_CTRL_BUF_NUM <\
 	CONFIG_BT_CTLR_LLCP_TX_PER_CONN_TX_CTRL_BUF_NUM_MAX)

subsys/bluetooth/controller/ll_sw/ull_df.c

@@ -1120,6 +1120,13 @@ uint8_t ll_df_set_conn_cte_rx_params(uint16_t handle, uint8_t sampling_enable,
 }
 #endif /* CONFIG_BT_CTLR_DF_CONN_CTE_RX */
 
+#if defined(CONFIG_BT_CTLR_DF_CONN_CTE_REQ) || defined(CONFIG_BT_CTLR_DF_CONN_CTE_RSP)
+static void df_conn_cte_req_disable(void *param)
+{
+	k_sem_give(param);
+}
+#endif /* CONFIG_BT_CTLR_DF_CONN_CTE_REQ || CONFIG_BT_CTLR_DF_CONN_CTE_RSP */
+
 #if defined(CONFIG_BT_CTLR_DF_CONN_CTE_REQ)
 /* @brief Function enables or disables CTE request control procedure for a connection.
  *
@@ -1156,10 +1163,18 @@ uint8_t ll_df_set_conn_cte_req_enable(uint16_t handle, uint8_t enable,
 		conn->llcp.cte_req.is_enabled = false;
 		conn->llcp.cte_req.req_interval = 0U;
 
-		/* There is no verification if the command is pending. If it is already disabled
+		if (conn->llcp.cte_req.is_active) {
-		 * there is no change to the state.
+			struct k_sem sem;
-		 */
+
-		/* TODO: How handle command pending in LLL? */
+			k_sem_init(&sem, 0U, 1U);
+			conn->llcp.cte_req.disable_param = &sem;
+			conn->llcp.cte_req.disable_cb = df_conn_cte_req_disable;
+
+			if (!conn->llcp.cte_req.is_active) {
+				k_sem_take(&sem, K_FOREVER);
+			}
+		}
+
 		return BT_HCI_ERR_SUCCESS;
 	} else {
 		if (!conn->lll.df_rx_cfg.is_initialized) {
@@ -1206,7 +1221,7 @@ uint8_t ll_df_set_conn_cte_req_enable(uint16_t handle, uint8_t enable,
 			return BT_HCI_ERR_UNSUPP_REMOTE_FEATURE;
 		}
 
-		conn->llcp.cte_req.is_enabled = true;
+		conn->llcp.cte_req.is_enabled = 0U;
 		conn->llcp.cte_req.req_interval = cte_request_interval;
 		conn->llcp.cte_req.cte_type = requested_cte_type;
 		conn->llcp.cte_req.min_cte_len = requested_cte_length;
@@ -1250,11 +1265,19 @@ uint8_t ll_df_set_conn_cte_rsp_enable(uint16_t handle, uint8_t enable)
 		ull_cp_cte_rsp_enable(conn, enable, LLL_DF_MAX_CTE_LEN,
 				conn->lll.df_tx_cfg.cte_types_allowed);
 	} else {
-		/* There is no parameter validation for disable operation. */
+		conn->lll.df_tx_cfg.cte_rsp_en = false;
 
-		/* TODO: Add missing implementation of disable CTE request.
+		if (conn->llcp.cte_rsp.is_active) {
-		 * Requires refactored LLCPs.
+			struct k_sem sem;
-		 */
+
+			k_sem_init(&sem, 0U, 1U);
+			conn->llcp.cte_rsp.disable_param = &sem;
+			conn->llcp.cte_rsp.disable_cb = df_conn_cte_req_disable;
+
+			if (!conn->llcp.cte_rsp.is_active) {
+				k_sem_take(&sem, K_FOREVER);
+			}
+		}
 	}
 
 	return BT_HCI_ERR_SUCCESS;

subsys/bluetooth/controller/ll_sw/ull_llcp.c

@@ -458,9 +458,15 @@ void ull_llcp_init(struct ll_conn *conn)
 #if defined(CONFIG_BT_CTLR_DF_CONN_CTE_REQ)
 	conn->llcp.cte_req.is_enabled = 0U;
 	conn->llcp.cte_req.req_expire = 0U;
+	conn->llcp.cte_req.is_active = 0U;
+	conn->llcp.cte_req.disable_param = NULL;
+	conn->llcp.cte_req.disable_cb = NULL;
 #endif /* CONFIG_BT_CTLR_DF_CONN_CTE_REQ */
 #if defined(CONFIG_BT_CTLR_DF_CONN_CTE_RSP)
 	conn->llcp.cte_rsp.is_enabled = 0U;
+	conn->llcp.cte_rsp.is_active = 0U;
+	conn->llcp.cte_rsp.disable_param = NULL;
+	conn->llcp.cte_rsp.disable_cb = NULL;
 #endif /* CONFIG_BT_CTLR_DF_CONN_CTE_RSP */
 
 #if defined(LLCP_TX_CTRL_BUF_QUEUE_ENABLE)
@@ -904,6 +910,9 @@ uint8_t ull_cp_cte_req(struct ll_conn *conn, uint8_t min_cte_len, uint8_t cte_ty
 
 	ctx->data.cte_req.min_len = min_cte_len;
 	ctx->data.cte_req.type = cte_type;
+
+	conn->llcp.cte_req.is_active = 1U;
+
 	llcp_lr_enqueue(conn, ctx);
 
 	return BT_HCI_ERR_SUCCESS;

subsys/bluetooth/controller/ll_sw/ull_llcp_common.c

@@ -356,18 +356,24 @@ static void lp_comm_complete(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t
 				conn->llcp.cte_req.is_enabled = 0U;
 				lp_comm_ntf(conn, ctx);
 			}
-			ctx->state = LP_COMMON_STATE_IDLE;
 		} else if (ctx->response_opcode == PDU_DATA_LLCTRL_TYPE_REJECT_EXT_IND &&
 			   ctx->reject_ext_ind.reject_opcode == PDU_DATA_LLCTRL_TYPE_CTE_REQ) {
 			lp_comm_ntf(conn, ctx);
 			conn->llcp.cte_req.is_enabled = 0U;
-			ctx->state = LP_COMMON_STATE_IDLE;
 		}
 
+		ctx->state = LP_COMMON_STATE_IDLE;
+
 		if (ctx->state == LP_COMMON_STATE_IDLE) {
 			llcp_rr_set_paused_cmd(conn, PROC_NONE);
 			llcp_lr_complete(conn);
 		}
+
+		conn->llcp.cte_req.is_active = 0U;
+		if (conn->llcp.cte_req.disable_cb) {
+			conn->llcp.cte_req.disable_cb(conn->llcp.cte_req.disable_param);
+		}
+
 		break;
 #endif /* CONFIG_BT_CTLR_DF_CONN_CTE_REQ */
 	default:
@@ -1008,6 +1014,7 @@ static void rp_comm_st_wait_tx_ack(struct ll_conn *conn, struct proc_ctx *ctx, u
 #if defined(CONFIG_BT_CTLR_DF_CONN_CTE_RSP)
 		case PROC_CTE_REQ: {
 			/* add PHY update pause = false here */
+			ctx->tx_ack = NULL;
 			llcp_rr_set_paused_cmd(conn, PROC_NONE);
 			llcp_rr_complete(conn);
 			ctx->state = RP_COMMON_STATE_IDLE;