Bluetooth: controller: Fix control tx queue handling (6991d099) · Commits · Wenxi XU / zephyr

subsys/bluetooth/controller/ll_sw/ctrl.c

+74 −9

Original line number	Diff line number	Diff line
		@@ -9925,10 +9925,17 @@ static void prepare_pdu_data_tx(struct connection *conn,

		if (!conn->pkt_tx_ctrl &&
		(conn->pkt_tx_head != conn->pkt_tx_data)) {
		struct pdu_data *pdu_data_tx;

		pdu_data_tx = (void *)conn->pkt_tx_head->pdu_data;
		if ((pdu_data_tx->ll_id != PDU_DATA_LLID_CTRL) \|\|
		(pdu_data_tx->llctrl.opcode !=
		PDU_DATA_LLCTRL_TYPE_ENC_REQ)) {
		conn->pkt_tx_ctrl = conn->pkt_tx_ctrl_last =
		conn->pkt_tx_head;
		}
		}
		}

		_pdu_data_tx->rfu = 0U;

		@@ -9947,8 +9954,9 @@ static void ctrl_tx_last_enqueue(struct connection *conn,
		conn->pkt_tx_ctrl_last = node_tx;
		}

		static void ctrl_tx_enqueue(struct connection *conn,
		struct radio_pdu_node_tx *node_tx)
		static inline void ctrl_tx_pause_enqueue(struct connection *conn,
		struct radio_pdu_node_tx *node_tx,
		bool pause)
		{
		/* check if a packet was tx-ed and not acked by peer */
		if (
		@@ -9976,9 +9984,27 @@ static void ctrl_tx_enqueue(struct connection *conn,
		if (!conn->pkt_tx_ctrl) {
		node_tx->next = conn->pkt_tx_head->next;
		conn->pkt_tx_head->next = node_tx;

		/* If in Encryption Procedure, other control PDUs,
		* Feature Rsp and Version Ind, are placed before data
		* marker and after control last marker. Hence, if no
		* control marker i.e. this is the first control PDU and
		* to be paused, do not set the control marker. A valid
		* control PDU in Encryption Procedure that is not
		* implicitly paused, will set the control and control
		* last marker.
		*/
		if (!pause) {
		conn->pkt_tx_ctrl = node_tx;
		conn->pkt_tx_ctrl_last = node_tx;
		}
		} else {
		/* ENC_REQ PDU is always allocated from data pool, hence
		* the head can not have the control marker, and pause
		* be true.
		*/
		LL_ASSERT(!pause);

		ctrl_tx_last_enqueue(conn, node_tx);
		}
		} else {
		@@ -9990,9 +10016,13 @@ static void ctrl_tx_enqueue(struct connection *conn,
		if (!conn->pkt_tx_ctrl) {
		node_tx->next = conn->pkt_tx_head;
		conn->pkt_tx_head = node_tx;
		if (!pause) {
		conn->pkt_tx_ctrl = node_tx;
		conn->pkt_tx_ctrl_last = node_tx;
		}
		} else {
		LL_ASSERT(!pause);

		ctrl_tx_last_enqueue(conn, node_tx);
		}
		}
		@@ -10003,19 +10033,54 @@ static void ctrl_tx_enqueue(struct connection *conn,
		}
		}

		static void ctrl_tx_enqueue(struct connection *conn,
		struct radio_pdu_node_tx *node_tx)
		{
		ctrl_tx_pause_enqueue(conn, node_tx, false);
		}

		static void ctrl_tx_sec_enqueue(struct connection *conn,
		struct radio_pdu_node_tx *node_tx)
		{
		if (conn->pause_tx) {
		if (!conn->pkt_tx_ctrl) {
		/* As data PDU tx is paused and no control PDU in queue,
		* its safe to add new control PDU at head.
		* Note, here the PDUs are stacked, not queued. Last In
		* First Out.
		*/
		node_tx->next = conn->pkt_tx_head;
		conn->pkt_tx_head = node_tx;
		} else {
		/* As data PDU tx is paused and there are control PDUs
		* in the queue, add it after control PDUs last marker
		* and before the data start marker.
		* Note, here the PDUs are stacked, not queued. Last In
		* First Out.
		*/
		node_tx->next = conn->pkt_tx_ctrl_last->next;
		conn->pkt_tx_ctrl_last->next = node_tx;
		}
		} else {
		ctrl_tx_enqueue(conn, node_tx);
		bool pause = false;

		/* check if Encryption Request is at head, it may have been
		* transmitted and not ack-ed. Hence, enqueue this control PDU
		* after control last marker and before data marker.
		* This way it is paused until Encryption Setup completes.
		*/
		if (conn->pkt_tx_head) {
		struct pdu_data *pdu_data_tx;

		pdu_data_tx = (void *)conn->pkt_tx_head->pdu_data;
		if ((pdu_data_tx->ll_id == PDU_DATA_LLID_CTRL) &&
		(pdu_data_tx->llctrl.opcode ==
		PDU_DATA_LLCTRL_TYPE_ENC_REQ)) {
		pause = true;
		}
		}

		ctrl_tx_pause_enqueue(conn, node_tx, pause);
		}
		}

Admin message