From bedbef8ad3e75a304af6361af235302bcc61d06b Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Tue, 14 May 2024 06:39:01 +0000
Subject: [PATCH] 修改内核路径
---
kernel/net/bluetooth/l2cap_core.c | 212 ++++++++++++++++++++++++++++++-----------------------
1 files changed, 120 insertions(+), 92 deletions(-)
diff --git a/kernel/net/bluetooth/l2cap_core.c b/kernel/net/bluetooth/l2cap_core.c
index e851f80..9cde366 100644
--- a/kernel/net/bluetooth/l2cap_core.c
+++ b/kernel/net/bluetooth/l2cap_core.c
@@ -710,6 +710,17 @@
}
EXPORT_SYMBOL_GPL(l2cap_chan_del);
+static void __l2cap_chan_list_id(struct l2cap_conn *conn, u16 id,
+ l2cap_chan_func_t func, void *data)
+{
+ struct l2cap_chan *chan, *l;
+
+ list_for_each_entry_safe(chan, l, &conn->chan_l, list) {
+ if (chan->ident == id)
+ func(chan, data);
+ }
+}
+
static void __l2cap_chan_list(struct l2cap_conn *conn, l2cap_chan_func_t func,
void *data)
{
@@ -777,23 +788,9 @@
static void l2cap_chan_ecred_connect_reject(struct l2cap_chan *chan)
{
- struct l2cap_conn *conn = chan->conn;
- struct l2cap_ecred_conn_rsp rsp;
- u16 result;
-
- if (test_bit(FLAG_DEFER_SETUP, &chan->flags))
- result = L2CAP_CR_LE_AUTHORIZATION;
- else
- result = L2CAP_CR_LE_BAD_PSM;
-
l2cap_state_change(chan, BT_DISCONN);
- memset(&rsp, 0, sizeof(rsp));
-
- rsp.result = cpu_to_le16(result);
-
- l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp),
- &rsp);
+ __l2cap_ecred_conn_rsp_defer(chan);
}
static void l2cap_chan_connect_reject(struct l2cap_chan *chan)
@@ -848,7 +845,7 @@
break;
case L2CAP_MODE_EXT_FLOWCTL:
l2cap_chan_ecred_connect_reject(chan);
- break;
+ return;
}
}
}
@@ -2679,14 +2676,6 @@
if (IS_ERR(skb))
return PTR_ERR(skb);
- /* Channel lock is released before requesting new skb and then
- * reacquired thus we need to recheck channel state.
- */
- if (chan->state != BT_CONNECTED) {
- kfree_skb(skb);
- return -ENOTCONN;
- }
-
l2cap_do_send(chan, skb);
return len;
}
@@ -2731,14 +2720,6 @@
if (IS_ERR(skb))
return PTR_ERR(skb);
- /* Channel lock is released before requesting new skb and then
- * reacquired thus we need to recheck channel state.
- */
- if (chan->state != BT_CONNECTED) {
- kfree_skb(skb);
- return -ENOTCONN;
- }
-
l2cap_do_send(chan, skb);
err = len;
break;
@@ -2758,14 +2739,6 @@
* allocation.
*/
err = l2cap_segment_sdu(chan, &seg_queue, msg, len);
-
- /* The channel could have been closed while segmenting,
- * check that it is still connected.
- */
- if (chan->state != BT_CONNECTED) {
- __skb_queue_purge(&seg_queue);
- err = -ENOTCONN;
- }
if (err)
break;
@@ -3958,43 +3931,86 @@
&rsp);
}
-void __l2cap_ecred_conn_rsp_defer(struct l2cap_chan *chan)
+static void l2cap_ecred_list_defer(struct l2cap_chan *chan, void *data)
{
- struct {
- struct l2cap_ecred_conn_rsp rsp;
- __le16 dcid[5];
- } __packed pdu;
- struct l2cap_conn *conn = chan->conn;
- u16 ident = chan->ident;
- int i = 0;
+ int *result = data;
- if (!ident)
+ if (*result || test_bit(FLAG_ECRED_CONN_REQ_SENT, &chan->flags))
return;
- BT_DBG("chan %p ident %d", chan, ident);
-
- pdu.rsp.mtu = cpu_to_le16(chan->imtu);
- pdu.rsp.mps = cpu_to_le16(chan->mps);
- pdu.rsp.credits = cpu_to_le16(chan->rx_credits);
- pdu.rsp.result = cpu_to_le16(L2CAP_CR_LE_SUCCESS);
-
- mutex_lock(&conn->chan_lock);
-
- list_for_each_entry(chan, &conn->chan_l, list) {
- if (chan->ident != ident)
- continue;
-
- /* Reset ident so only one response is sent */
- chan->ident = 0;
-
- /* Include all channels pending with the same ident */
- pdu.dcid[i++] = cpu_to_le16(chan->scid);
+ switch (chan->state) {
+ case BT_CONNECT2:
+ /* If channel still pending accept add to result */
+ (*result)++;
+ return;
+ case BT_CONNECTED:
+ return;
+ default:
+ /* If not connected or pending accept it has been refused */
+ *result = -ECONNREFUSED;
+ return;
}
+}
- mutex_unlock(&conn->chan_lock);
+struct l2cap_ecred_rsp_data {
+ struct {
+ struct l2cap_ecred_conn_rsp rsp;
+ __le16 scid[L2CAP_ECRED_MAX_CID];
+ } __packed pdu;
+ int count;
+};
- l2cap_send_cmd(conn, ident, L2CAP_ECRED_CONN_RSP,
- sizeof(pdu.rsp) + i * sizeof(__le16), &pdu);
+static void l2cap_ecred_rsp_defer(struct l2cap_chan *chan, void *data)
+{
+ struct l2cap_ecred_rsp_data *rsp = data;
+
+ if (test_bit(FLAG_ECRED_CONN_REQ_SENT, &chan->flags))
+ return;
+
+ /* Reset ident so only one response is sent */
+ chan->ident = 0;
+
+ /* Include all channels pending with the same ident */
+ if (!rsp->pdu.rsp.result)
+ rsp->pdu.rsp.dcid[rsp->count++] = cpu_to_le16(chan->scid);
+ else
+ l2cap_chan_del(chan, ECONNRESET);
+}
+
+void __l2cap_ecred_conn_rsp_defer(struct l2cap_chan *chan)
+{
+ struct l2cap_conn *conn = chan->conn;
+ struct l2cap_ecred_rsp_data data;
+ u16 id = chan->ident;
+ int result = 0;
+
+ if (!id)
+ return;
+
+ BT_DBG("chan %p id %d", chan, id);
+
+ memset(&data, 0, sizeof(data));
+
+ data.pdu.rsp.mtu = cpu_to_le16(chan->imtu);
+ data.pdu.rsp.mps = cpu_to_le16(chan->mps);
+ data.pdu.rsp.credits = cpu_to_le16(chan->rx_credits);
+ data.pdu.rsp.result = cpu_to_le16(L2CAP_CR_LE_SUCCESS);
+
+ /* Verify that all channels are ready */
+ __l2cap_chan_list_id(conn, id, l2cap_ecred_list_defer, &result);
+
+ if (result > 0)
+ return;
+
+ if (result < 0)
+ data.pdu.rsp.result = cpu_to_le16(L2CAP_CR_LE_AUTHORIZATION);
+
+ /* Build response */
+ __l2cap_chan_list_id(conn, id, l2cap_ecred_rsp_defer, &data);
+
+ l2cap_send_cmd(conn, id, L2CAP_ECRED_CONN_RSP,
+ sizeof(data.pdu.rsp) + (data.count * sizeof(__le16)),
+ &data.pdu);
}
void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
@@ -4287,6 +4303,10 @@
result = __le16_to_cpu(rsp->result);
status = __le16_to_cpu(rsp->status);
+ if (result == L2CAP_CR_SUCCESS && (dcid < L2CAP_CID_DYN_START ||
+ dcid > L2CAP_CID_DYN_END))
+ return -EPROTO;
+
BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
dcid, scid, result, status);
@@ -4318,6 +4338,11 @@
switch (result) {
case L2CAP_CR_SUCCESS:
+ if (__l2cap_get_chan_by_dcid(conn, dcid)) {
+ err = -EBADSLT;
+ break;
+ }
+
l2cap_state_change(chan, BT_CONFIG);
chan->ident = 0;
chan->dcid = dcid;
@@ -4631,17 +4656,11 @@
BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid);
- mutex_lock(&conn->chan_lock);
-
- chan = __l2cap_get_chan_by_scid(conn, dcid);
+ chan = l2cap_get_chan_by_scid(conn, dcid);
if (!chan) {
- mutex_unlock(&conn->chan_lock);
cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid);
return 0;
}
-
- l2cap_chan_hold(chan);
- l2cap_chan_lock(chan);
rsp.dcid = cpu_to_le16(chan->scid);
rsp.scid = cpu_to_le16(chan->dcid);
@@ -4649,14 +4668,16 @@
chan->ops->set_shutdown(chan);
+ l2cap_chan_unlock(chan);
+ mutex_lock(&conn->chan_lock);
+ l2cap_chan_lock(chan);
l2cap_chan_del(chan, ECONNRESET);
+ mutex_unlock(&conn->chan_lock);
chan->ops->close(chan);
l2cap_chan_unlock(chan);
l2cap_chan_put(chan);
-
- mutex_unlock(&conn->chan_lock);
return 0;
}
@@ -4677,32 +4698,27 @@
BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid);
- mutex_lock(&conn->chan_lock);
-
- chan = __l2cap_get_chan_by_scid(conn, scid);
+ chan = l2cap_get_chan_by_scid(conn, scid);
if (!chan) {
- mutex_unlock(&conn->chan_lock);
return 0;
}
-
- l2cap_chan_hold(chan);
- l2cap_chan_lock(chan);
if (chan->state != BT_DISCONN) {
l2cap_chan_unlock(chan);
l2cap_chan_put(chan);
- mutex_unlock(&conn->chan_lock);
return 0;
}
+ l2cap_chan_unlock(chan);
+ mutex_lock(&conn->chan_lock);
+ l2cap_chan_lock(chan);
l2cap_chan_del(chan, 0);
+ mutex_unlock(&conn->chan_lock);
chan->ops->close(chan);
l2cap_chan_unlock(chan);
l2cap_chan_put(chan);
-
- mutex_unlock(&conn->chan_lock);
return 0;
}
@@ -5976,7 +5992,7 @@
struct l2cap_ecred_conn_req *req = (void *) data;
struct {
struct l2cap_ecred_conn_rsp rsp;
- __le16 dcid[5];
+ __le16 dcid[L2CAP_ECRED_MAX_CID];
} __packed pdu;
struct l2cap_chan *chan, *pchan;
u16 mtu, mps;
@@ -5989,6 +6005,14 @@
return -EINVAL;
if (cmd_len < sizeof(*req) || (cmd_len - sizeof(*req)) % sizeof(u16)) {
+ result = L2CAP_CR_LE_INVALID_PARAMS;
+ goto response;
+ }
+
+ cmd_len -= sizeof(*req);
+ num_scid = cmd_len / sizeof(u16);
+
+ if (num_scid > ARRAY_SIZE(pdu.dcid)) {
result = L2CAP_CR_LE_INVALID_PARAMS;
goto response;
}
@@ -6037,8 +6061,6 @@
}
result = L2CAP_CR_LE_SUCCESS;
- cmd_len -= sizeof(*req);
- num_scid = cmd_len / sizeof(u16);
for (i = 0; i < num_scid; i++) {
u16 scid = __le16_to_cpu(req->scid[i]);
@@ -6091,6 +6113,7 @@
__set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
chan->ident = cmd->ident;
+ chan->mode = L2CAP_MODE_EXT_FLOWCTL;
if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
l2cap_state_change(chan, BT_CONNECT2);
@@ -6347,9 +6370,14 @@
if (!chan)
goto done;
+ chan = l2cap_chan_hold_unless_zero(chan);
+ if (!chan)
+ goto done;
+
l2cap_chan_lock(chan);
l2cap_chan_del(chan, ECONNREFUSED);
l2cap_chan_unlock(chan);
+ l2cap_chan_put(chan);
done:
mutex_unlock(&conn->chan_lock);
--
Gitblit v1.6.2