From 6991d09977ba5056ca3ec5a004941aa893b85585 Mon Sep 17 00:00:00 2001 From: Vinayak Kariappa Chettimada Date: Wed, 4 Sep 2019 11:29:54 +0530 Subject: [PATCH] Bluetooth: controller: Fix control tx queue handling Fix control tx queue handling to correctly pause control PDU responses during encryption setup. Signed-off-by: Vinayak Kariappa Chettimada --- subsys/bluetooth/controller/ll_sw/ctrl.c | 83 +++++++++++++++++++++--- 1 file changed, 74 insertions(+), 9 deletions(-) diff --git a/subsys/bluetooth/controller/ll_sw/ctrl.c b/subsys/bluetooth/controller/ll_sw/ctrl.c index 63fbfb20c13..c223cac5f84 100644 --- a/subsys/bluetooth/controller/ll_sw/ctrl.c +++ b/subsys/bluetooth/controller/ll_sw/ctrl.c @@ -9925,8 +9925,15 @@ static void prepare_pdu_data_tx(struct connection *conn, if (!conn->pkt_tx_ctrl && (conn->pkt_tx_head != conn->pkt_tx_data)) { - conn->pkt_tx_ctrl = conn->pkt_tx_ctrl_last = - 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)) { + conn->pkt_tx_ctrl = conn->pkt_tx_ctrl_last = + conn->pkt_tx_head; + } } } @@ -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; - conn->pkt_tx_ctrl = node_tx; - conn->pkt_tx_ctrl_last = 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; - conn->pkt_tx_ctrl = node_tx; - conn->pkt_tx_ctrl_last = 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); } }