Bluetooth: llcp: Fix PHY UPD ntf send before instant on air

PHY update control procedure has to send a notification towards
Host when the PHY is actually changed, when the instant happens.
Control procedures are handled in a prepare phase of a connection
event, hence notifications are send towards host to early.

The fix for that is to postpone send of a notification.
New state has been introduced to remote requested PHY update
control procedure: RP_PU_STATE_WAIT_INSTANT_ON_AIR.
Also new event has been introduced: RP_PU_EVT_NTF, that is used
to inform the PHY update state machine that notification may
be send.

There are two places where the notification events may be generated
ull_conn_rx, just before first received PDU is send towards Host
or ull_conn_done, in case there were no PDU received.

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

subsys/bluetooth/controller/ll_sw/ull_conn.c

@@ -1066,6 +1066,10 @@ int ull_conn_rx(memq_link_t *link, struct node_rx_pdu **rx)
 	}
 #endif /* CONFIG_BT_CTLR_RX_ENQUEUE_HOLD */
 
+#if !defined(CONFIG_BT_LL_SW_LLCP_LEGACY)
+	ull_cp_tx_ntf(conn);
+#endif /* CONFIG_BT_LL_SW_LLCP_LEGACY */
+
 	pdu_rx = (void *)(*rx)->pdu;
 
 	switch (pdu_rx->ll_id) {
@@ -1456,6 +1460,10 @@ void ull_conn_done(struct node_rx_event_done *done)
 	}
 #endif /* CONFIG_BT_CTLR_RX_ENQUEUE_HOLD */
 
+#if !defined(CONFIG_BT_LL_SW_LLCP_LEGACY)
+	ull_cp_tx_ntf(conn);
+#endif /* CONFIG_BT_LL_SW_LLCP_LEGACY */
+
 #if defined(CONFIG_BT_CTLR_LE_ENC)
 	/* Check authenticated payload expiry or MIC failure */
 	switch (done->extra.mic_state) {

subsys/bluetooth/controller/ll_sw/ull_llcp.c

@@ -1570,6 +1570,23 @@ void ull_cp_tx_ack(struct ll_conn *conn, struct node_tx *tx)
 	}
 }
 
+void ull_cp_tx_ntf(struct ll_conn *conn)
+{
+	struct proc_ctx *ctx;
+
+	ctx = llcp_lr_peek(conn);
+	if (ctx) {
+		/* TX notifications towards Host */
+		llcp_lr_tx_ntf(conn, ctx);
+	}
+
+	ctx = llcp_rr_peek(conn);
+	if (ctx) {
+		/* TX notifications towards Host */
+		llcp_rr_tx_ntf(conn, ctx);
+	}
+}
+
 void ull_cp_rx(struct ll_conn *conn, struct node_rx_pdu *rx)
 {
 	struct proc_ctx *ctx_l;

subsys/bluetooth/controller/ll_sw/ull_llcp.h

@@ -60,6 +60,11 @@ void ull_cp_run(struct ll_conn *conn);
  */
 void ull_cp_tx_ack(struct ll_conn *conn, struct node_tx *tx);
 
+/**
+ * @brief Handle TX procedures notifications towards Host.
+ */
+void ull_cp_tx_ntf(struct ll_conn *conn);
+
 /**
  * @brief Handle received LL Control PDU.
  */

subsys/bluetooth/controller/ll_sw/ull_llcp_internal.h

@@ -449,6 +449,7 @@ void llcp_lp_pu_rx(struct ll_conn *conn, struct proc_ctx *ctx, struct node_rx_pd
 void llcp_lp_pu_init_proc(struct proc_ctx *ctx);
 void llcp_lp_pu_run(struct ll_conn *conn, struct proc_ctx *ctx, void *param);
 void llcp_lp_pu_tx_ack(struct ll_conn *conn, struct proc_ctx *ctx, void *param);
+void llcp_lp_pu_tx_ntf(struct ll_conn *conn, struct proc_ctx *ctx);
 #endif /* CONFIG_BT_CTLR_PHY */
 
 /*
@@ -473,6 +474,7 @@ void llcp_rp_pu_rx(struct ll_conn *conn, struct proc_ctx *ctx, struct node_rx_pd
 void llcp_rp_pu_init_proc(struct proc_ctx *ctx);
 void llcp_rp_pu_run(struct ll_conn *conn, struct proc_ctx *ctx, void *param);
 void llcp_rp_pu_tx_ack(struct ll_conn *conn, struct proc_ctx *ctx, void *param);
+void llcp_rp_pu_tx_ntf(struct ll_conn *conn, struct proc_ctx *ctx);
 #endif /* CONFIG_BT_CTLR_PHY */
 
 /*
@@ -499,6 +501,7 @@ bool llcp_lr_ispaused(struct ll_conn *conn);
 void llcp_lr_pause(struct ll_conn *conn);
 void llcp_lr_resume(struct ll_conn *conn);
 void llcp_lr_tx_ack(struct ll_conn *conn, struct proc_ctx *ctx, struct node_tx *tx);
+void llcp_lr_tx_ntf(struct ll_conn *conn, struct proc_ctx *ctx);
 void llcp_lr_rx(struct ll_conn *conn, struct proc_ctx *ctx, struct node_rx_pdu *rx);
 void llcp_lr_enqueue(struct ll_conn *conn, struct proc_ctx *ctx);
 void llcp_lr_init(struct ll_conn *conn);
@@ -520,6 +523,7 @@ bool llcp_rr_ispaused(struct ll_conn *conn);
 void llcp_rr_pause(struct ll_conn *conn);
 void llcp_rr_resume(struct ll_conn *conn);
 void llcp_rr_tx_ack(struct ll_conn *conn, struct proc_ctx *ctx, struct node_tx *tx);
+void llcp_rr_tx_ntf(struct ll_conn *conn, struct proc_ctx *ctx);
 void llcp_rr_rx(struct ll_conn *conn, struct proc_ctx *ctx, struct node_rx_pdu *rx);
 void llcp_rr_init(struct ll_conn *conn);
 void llcp_rr_prepare(struct ll_conn *conn, struct node_rx_pdu *rx);

subsys/bluetooth/controller/ll_sw/ull_llcp_local.c

@@ -243,6 +243,22 @@ void llcp_lr_tx_ack(struct ll_conn *conn, struct proc_ctx *ctx, struct node_tx *
 	lr_check_done(conn, ctx);
 }
 
+void llcp_lr_tx_ntf(struct ll_conn *conn, struct proc_ctx *ctx)
+{
+	switch (ctx->proc) {
+#if defined(CONFIG_BT_CTLR_PHY)
+	case PROC_PHY_UPDATE:
+		llcp_lp_pu_tx_ntf(conn, ctx);
+		break;
+#endif /* CONFIG_BT_CTLR_PHY */
+	default:
+		/* Ignore other procedures */
+		break;
+	}
+
+	lr_check_done(conn, ctx);
+}
+
 static void lr_act_run(struct ll_conn *conn)
 {
 	struct proc_ctx *ctx;

subsys/bluetooth/controller/ll_sw/ull_llcp_phy.c

@@ -58,6 +58,7 @@ enum {
 	LP_PU_STATE_WAIT_TX_ACK_PHY_UPDATE_IND,
 	LP_PU_STATE_WAIT_RX_PHY_UPDATE_IND,
 	LP_PU_STATE_WAIT_INSTANT,
+	LP_PU_STATE_WAIT_INSTANT_ON_AIR,
 	LP_PU_STATE_WAIT_NTF,
 };
 
@@ -75,6 +76,9 @@ enum {
 	/* Ack received */
 	LP_PU_EVT_ACK,
 
+	/* Ready to notify host */
+	LP_PU_EVT_NTF,
+
 	/* Reject response received */
 	LP_PU_EVT_REJECT,
 
@@ -92,6 +96,7 @@ enum {
 	RP_PU_STATE_WAIT_TX_ACK_PHY_UPDATE_IND,
 	RP_PU_STATE_WAIT_RX_PHY_UPDATE_IND,
 	RP_PU_STATE_WAIT_INSTANT,
+	RP_PU_STATE_WAIT_INSTANT_ON_AIR,
 	RP_PU_STATE_WAIT_NTF,
 };
 
@@ -108,6 +113,9 @@ enum {
 
 	/* Indication received */
 	RP_PU_EVT_PHY_UPDATE_IND,
+
+	/* Ready to notify host */
+	RP_PU_EVT_NTF,
 };
 
 /* Hardcoded instant delta +6 */
@@ -443,7 +451,7 @@ static void pu_dle_ntf(struct ll_conn *conn, struct proc_ctx *ctx)
 }
 #endif
 
-static void lp_pu_complete(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t evt, void *param)
+static void lp_pu_tx_ntf(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t evt)
 {
 #if defined(CONFIG_BT_CTLR_DATA_LENGTH)
 #define NTF_DLE (ctx->data.pu.ntf_dle)
@@ -451,11 +459,6 @@ static void lp_pu_complete(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t e
 #define NTF_DLE 0
 #endif
 	uint8_t ntf_count = ctx->data.pu.ntf_pu + NTF_DLE;
-	/* when complete reset timing restrictions - idempotent
-	 * (so no problem if we need to wait for NTF buffer)
-	 */
-
-	pu_reset_timing_restrict(conn);
 
 	/* if we need to send both PHY and DLE notification, but we
 	 * do not have 2 buffers available we serialize the sending
@@ -489,6 +492,28 @@ static void lp_pu_complete(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t e
 	}
 }
 
+static void lp_pu_complete_after_inst_on_air(struct ll_conn *conn, struct proc_ctx *ctx,
+					     uint8_t evt, void *param)
+{
+	/* When complete reset timing restrictions - idempotent
+	 * (so no problem if we need to wait for NTF buffer)
+	 */
+	pu_reset_timing_restrict(conn);
+
+	/* Wait for instant on air to send notification */
+	ctx->state = LP_PU_STATE_WAIT_INSTANT_ON_AIR;
+}
+
+static void lp_pu_complete(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t evt, void *param)
+{
+	/* when complete reset timing restrictions - idempotent
+	 * (so no problem if we need to wait for NTF buffer)
+	 */
+	pu_reset_timing_restrict(conn);
+
+	lp_pu_tx_ntf(conn, ctx, evt);
+}
+
 static void lp_pu_send_phy_req(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t evt, void *param)
 {
 	if (llcp_lr_ispaused(conn) || llcp_rr_get_collision(conn) ||
@@ -721,7 +746,7 @@ static void lp_pu_check_instant(struct ll_conn *conn, struct proc_ctx *ctx, uint
 		llcp_rr_set_incompat(conn, INCOMPAT_NO_COLLISION);
 		ctx->data.pu.error = BT_HCI_ERR_SUCCESS;
 		ctx->data.pu.ntf_pu = (phy_changed || ctx->data.pu.host_initiated);
-		lp_pu_complete(conn, ctx, evt, param);
+		lp_pu_complete_after_inst_on_air(conn, ctx, evt, param);
 	}
 }
 
@@ -738,11 +763,23 @@ static void lp_pu_st_wait_instant(struct ll_conn *conn, struct proc_ctx *ctx, ui
 	}
 }
 
+static void lp_pu_st_wait_instant_on_air(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t evt)
+{
+	switch (evt) {
+	case LP_PU_EVT_NTF:
+		lp_pu_tx_ntf(conn, ctx, evt);
+		break;
+	default:
+		/* Ignore other evts */
+		break;
+	}
+}
+
 static void lp_pu_st_wait_ntf(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t evt, void *param)
 {
 	switch (evt) {
 	case LP_PU_EVT_RUN:
-		lp_pu_complete(conn, ctx, evt, param);
+		lp_pu_tx_ntf(conn, ctx, evt);
 		break;
 	default:
 		/* Ignore other evts */
@@ -781,6 +818,9 @@ static void lp_pu_execute_fsm(struct ll_conn *conn, struct proc_ctx *ctx, uint8_
 	case LP_PU_STATE_WAIT_INSTANT:
 		lp_pu_st_wait_instant(conn, ctx, evt, param);
 		break;
+	case LP_PU_STATE_WAIT_INSTANT_ON_AIR:
+		lp_pu_st_wait_instant_on_air(conn, ctx, evt);
+		break;
 	case LP_PU_STATE_WAIT_NTF:
 		lp_pu_st_wait_ntf(conn, ctx, evt, param);
 		break;
@@ -836,6 +876,10 @@ void llcp_lp_pu_tx_ack(struct ll_conn *conn, struct proc_ctx *ctx, void *param)
 	lp_pu_execute_fsm(conn, ctx, LP_PU_EVT_ACK, param);
 }
 
+void llcp_lp_pu_tx_ntf(struct ll_conn *conn, struct proc_ctx *ctx)
+{
+	lp_pu_execute_fsm(conn, ctx, LP_PU_EVT_NTF, NULL);
+}
 /*
  * LLCP Remote Procedure PHY Update FSM
  */
@@ -878,18 +922,38 @@ static void rp_pu_tx(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t opcode)
 	llcp_rr_prt_restart(conn);
 }
 
+static void rp_pu_complete_finalize(struct ll_conn *conn, struct proc_ctx *ctx)
+{
+	llcp_rr_set_paused_cmd(conn, PROC_NONE);
+	llcp_rr_complete(conn);
+	ctx->state = RP_PU_STATE_IDLE;
+}
+
 static void rp_pu_complete(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t evt, void *param)
 {
+	/* when complete reset timing restrictions - idempotent
+	 * (so no problem if we need to wait for NTF buffer)
+	 */
+	pu_reset_timing_restrict(conn);
+
+	/* For remote initiated PHY update Host is notified only if a PHY changes */
+	if (ctx->data.pu.ntf_pu) {
+		/* Notification may be send after instant is on air */
+		ctx->state = RP_PU_STATE_WAIT_INSTANT_ON_AIR;
+	} else {
+		rp_pu_complete_finalize(conn, ctx);
+	}
+}
+
+void rp_pu_tx_ntf(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t evt, void *param)
+{
+
 #if defined(CONFIG_BT_CTLR_DATA_LENGTH)
 #define NTF_DLE (ctx->data.pu.ntf_dle)
 #else
 #define NTF_DLE 0
 #endif
 	uint8_t ntf_count = ctx->data.pu.ntf_pu + NTF_DLE;
-	/* when complete reset timing restrictions - idempotent
-	 * (so no problem if we need to wait for NTF buffer)
-	 */
-	pu_reset_timing_restrict(conn);
 
 	/* if we need to send both PHY and DLE notification, but we
 	 * do not have 2 buffers available we serialize the sending
@@ -917,9 +981,7 @@ static void rp_pu_complete(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t e
 			pu_dle_ntf(conn, ctx);
 		}
 #endif
-		llcp_rr_set_paused_cmd(conn, PROC_NONE);
+		rp_pu_complete_finalize(conn, ctx);
-		llcp_rr_complete(conn);
-		ctx->state = RP_PU_STATE_IDLE;
 	}
 }
 
@@ -1134,11 +1196,24 @@ static void rp_pu_st_wait_instant(struct ll_conn *conn, struct proc_ctx *ctx, ui
 	}
 }
 
+static void rp_pu_st_wait_instant_on_air(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t evt,
+					 void *param)
+{
+	switch (evt) {
+	case RP_PU_EVT_NTF:
+		rp_pu_tx_ntf(conn, ctx, evt, param);
+		break;
+	default:
+		/* Ignore other evts */
+		break;
+	}
+}
+
 static void rp_pu_st_wait_ntf(struct ll_conn *conn, struct proc_ctx *ctx, uint8_t evt, void *param)
 {
 	switch (evt) {
 	case RP_PU_EVT_RUN:
-		rp_pu_complete(conn, ctx, evt, param);
+		rp_pu_tx_ntf(conn, ctx, evt, param);
 		break;
 	default:
 		/* Ignore other evts */
@@ -1177,6 +1252,9 @@ static void rp_pu_execute_fsm(struct ll_conn *conn, struct proc_ctx *ctx, uint8_
 	case RP_PU_STATE_WAIT_INSTANT:
 		rp_pu_st_wait_instant(conn, ctx, evt, param);
 		break;
+	case RP_PU_STATE_WAIT_INSTANT_ON_AIR:
+		rp_pu_st_wait_instant_on_air(conn, ctx, evt, param);
+		break;
 	case RP_PU_STATE_WAIT_NTF:
 		rp_pu_st_wait_ntf(conn, ctx, evt, param);
 		break;
@@ -1223,3 +1301,8 @@ void llcp_rp_pu_tx_ack(struct ll_conn *conn, struct proc_ctx *ctx, void *param)
 {
 	rp_pu_execute_fsm(conn, ctx, RP_PU_EVT_ACK, param);
 }
+
+void llcp_rp_pu_tx_ntf(struct ll_conn *conn, struct proc_ctx *ctx)
+{
+	rp_pu_execute_fsm(conn, ctx, RP_PU_EVT_NTF, NULL);
+}

subsys/bluetooth/controller/ll_sw/ull_llcp_remote.c

@@ -312,6 +312,27 @@ void llcp_rr_tx_ack(struct ll_conn *conn, struct proc_ctx *ctx, struct node_tx *
 	rr_check_done(conn, ctx);
 }
 
+void llcp_rr_tx_ntf(struct ll_conn *conn, struct proc_ctx *ctx)
+{
+	switch (ctx->proc) {
+#if defined(CONFIG_BT_CTLR_DATA_LENGTH)
+	case PROC_DATA_LENGTH_UPDATE:
+		/* llcp_rp_comm_tx_ntf(conn, ctx); */
+		break;
+#endif /* CONFIG_BT_CTLR_DATA_LENGTH */
+#ifdef CONFIG_BT_CTLR_PHY
+	case PROC_PHY_UPDATE:
+		llcp_rp_pu_tx_ntf(conn, ctx);
+		break;
+#endif /* CONFIG_BT_CTLR_PHY */
+	default:
+		/* Ignore other procedures */
+		break;
+	}
+
+	rr_check_done(conn, ctx);
+}
+
 static void rr_act_run(struct ll_conn *conn)
 {
 	struct proc_ctx *ctx;