BLE Host: Send invalid CID l2cap sig err.

4 files changed, 139 insertions, 103 deletions

diff --git a/net/nimble/host/src/ble_l2cap.c b/net/nimble/host/src/ble_l2cap.c
index 65b3fc2c..f1921f41 100644
--- a/net/nimble/host/src/ble_l2cap.c
+++ b/net/nimble/host/src/ble_l2cap.c
@@ -212,6 +212,12 @@ ble_l2cap_rx(struct ble_hs_conn *conn,
             BLE_HS_LOG(DEBUG, "rx on unknown L2CAP channel: %d\n",
                        l2cap_hdr.blh_cid);
             rc = BLE_HS_ENOENT;
+
+            chan = ble_hs_conn_chan_find(conn, BLE_L2CAP_CID_SIG);
+            if (chan != NULL) {
+                ble_l2cap_sig_reject_invalid_cid_tx(conn, chan, 0, 0,
+                                                    l2cap_hdr.blh_cid);
+            }
             goto err;
         }
 
diff --git a/net/nimble/host/src/ble_l2cap_sig.c b/net/nimble/host/src/ble_l2cap_sig.c
index ff98426b..13c830b9 100644
--- a/net/nimble/host/src/ble_l2cap_sig.c
+++ b/net/nimble/host/src/ble_l2cap_sig.c
@@ -109,18 +109,6 @@ static struct os_mempool ble_l2cap_sig_proc_pool;
  * $misc                                                                     *
  *****************************************************************************/
 
-static int
-ble_l2cap_sig_conn_chan_find(uint16_t conn_handle,
-                             struct ble_hs_conn **out_conn,
-                             struct ble_l2cap_chan **out_chan)
-{
-    int rc;
-
-    rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
-                                         out_conn, out_chan);
-    return rc;
-}
-
 static uint8_t
 ble_l2cap_sig_next_id(void)
 {
@@ -285,64 +273,68 @@ ble_l2cap_sig_update_req_rx(uint16_t conn_handle,
     struct ble_l2cap_sig_update_req req;
     struct ble_gap_upd_params params;
     struct ble_l2cap_chan *chan;
+    ble_hs_conn_flags_t conn_flags;
     struct ble_hs_conn *conn;
     uint16_t l2cap_result;
-    int is_master;
+    int sig_err;
     int rc;
 
+    l2cap_result = 0; /* Silence spurious gcc warning. */
+
     rc = ble_hs_misc_pullup_base(om, BLE_L2CAP_SIG_UPDATE_REQ_SZ);
     if (rc != 0) {
         return rc;
     }
 
-    ble_hs_lock();
-    rc = ble_l2cap_sig_conn_chan_find(conn_handle, &conn, &chan);
-    if (rc == 0) {
-        is_master = conn->bhc_flags & BLE_HS_CONN_F_MASTER;
-    }
-    ble_hs_unlock();
-
+    rc = ble_hs_atomic_conn_flags(conn_handle, &conn_flags);
     if (rc != 0) {
         return rc;
     }
 
     /* Only a master can process an update request. */
-    if (!is_master) {
-        ble_l2cap_sig_reject_tx(conn_handle, hdr->identifier,
-                                BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD);
-        return BLE_HS_ENOTSUP;
-    }
-
-    ble_l2cap_sig_update_req_parse((*om)->om_data, (*om)->om_len, &req);
-
-    params.itvl_min = req.itvl_min;
-    params.itvl_max = req.itvl_max;
-    params.latency = req.slave_latency;
-    params.supervision_timeout = req.timeout_multiplier;
-    params.min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN;
-    params.max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN;
-
-    /* Ask application if slave's connection parameters are acceptable. */
-    rc = ble_gap_rx_l2cap_update_req(conn_handle, &params);
-    if (rc == 0) {
-        /* Application agrees to accept parameters; schedule update. */
-        rc = ble_gap_update_params(conn_handle, &params);
-        if (rc != 0) {
-            return rc;
+    sig_err = !(conn_flags & BLE_HS_CONN_F_MASTER);
+    if (!sig_err) {
+        ble_l2cap_sig_update_req_parse((*om)->om_data, (*om)->om_len, &req);
+
+        params.itvl_min = req.itvl_min;
+        params.itvl_max = req.itvl_max;
+        params.latency = req.slave_latency;
+        params.supervision_timeout = req.timeout_multiplier;
+        params.min_ce_len = BLE_GAP_INITIAL_CONN_MIN_CE_LEN;
+        params.max_ce_len = BLE_GAP_INITIAL_CONN_MAX_CE_LEN;
+
+        /* Ask application if slave's connection parameters are acceptable. */
+        rc = ble_gap_rx_l2cap_update_req(conn_handle, &params);
+        if (rc == 0) {
+            /* Application agrees to accept parameters; schedule update. */
+            rc = ble_gap_update_params(conn_handle, &params);
+            if (rc != 0) {
+                return rc;
+            }
+            l2cap_result = BLE_L2CAP_SIG_UPDATE_RSP_RESULT_ACCEPT;
+        } else {
+            l2cap_result = BLE_L2CAP_SIG_UPDATE_RSP_RESULT_REJECT;
         }
-        l2cap_result = BLE_L2CAP_SIG_UPDATE_RSP_RESULT_ACCEPT;
-    } else {
-        l2cap_result = BLE_L2CAP_SIG_UPDATE_RSP_RESULT_REJECT;
     }
 
     /* Send L2CAP response. */
-    rc = ble_l2cap_sig_update_rsp_tx(conn_handle, hdr->identifier,
-                                     l2cap_result);
-    if (rc != 0) {
-        return rc;
+    ble_hs_lock();
+    rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
+                                         &conn, &chan);
+    if (rc == 0) {
+        if (!sig_err) {
+            rc = ble_l2cap_sig_update_rsp_tx(conn, chan, hdr->identifier,
+                                             l2cap_result);
+        } else {
+            ble_l2cap_sig_reject_tx(conn, chan, hdr->identifier,
+                                    BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD,
+                                    NULL, 0);
+            rc = BLE_HS_L2C_ERR(BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD);
+        }
     }
+    ble_hs_unlock();
 
-    return 0;
+    return rc;
 }
 
 static int
@@ -400,30 +392,34 @@ ble_l2cap_sig_update(uint16_t conn_handle,
 {
     struct ble_l2cap_sig_update_req req;
     struct ble_l2cap_sig_proc *proc;
-    ble_hs_conn_flags_t conn_flags;
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
     int rc;
 
     proc = NULL;
 
     STATS_INC(ble_l2cap_stats, update_init);
 
-    rc = ble_hs_atomic_conn_flags(conn_handle, &conn_flags);
+    ble_hs_lock();
+
+    rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
+                                         &conn, &chan);
     if (rc != 0) {
-        return rc;
+        goto done;
     }
-    if (conn_flags & BLE_HS_CONN_F_MASTER) {
+    if (conn->bhc_flags & BLE_HS_CONN_F_MASTER) {
         /* Only the slave can initiate the L2CAP connection update
          * procedure.
          */
         rc = BLE_HS_EINVAL;
-        goto err;
+        goto done;
     }
 
     proc = ble_l2cap_sig_proc_alloc();
     if (proc == NULL) {
         STATS_INC(ble_l2cap_stats, update_fail);
         rc = BLE_HS_ENOMEM;
-        goto err;
+        goto done;
     }
 
     proc->op = BLE_L2CAP_SIG_PROC_OP_UPDATE;
@@ -438,17 +434,18 @@ ble_l2cap_sig_update(uint16_t conn_handle,
     req.slave_latency = params->slave_latency;
     req.timeout_multiplier = params->timeout_multiplier;
 
-    rc = ble_l2cap_sig_update_req_tx(conn_handle, proc->id, &req);
-    if (rc != 0) {
-        goto err;
+    rc = ble_l2cap_sig_update_req_tx(conn, chan, proc->id, &req);
+    if (rc == 0) {
+        ble_l2cap_sig_proc_insert(proc);
     }
 
-    ble_l2cap_sig_proc_insert(proc);
+done:
+    ble_hs_unlock();
 
-    return 0;
+    if (rc != 0) {
+        ble_l2cap_sig_proc_free(proc);
+    }
 
-err:
-    ble_l2cap_sig_proc_free(proc);
     return rc;
 }
 
@@ -456,6 +453,8 @@ static int
 ble_l2cap_sig_rx(uint16_t conn_handle, struct os_mbuf **om)
 {
     struct ble_l2cap_sig_hdr hdr;
+    struct ble_l2cap_chan *chan;
+    struct ble_hs_conn *conn;
     ble_l2cap_sig_rx_fn *rx_cb;
     int rc;
 
@@ -480,9 +479,18 @@ ble_l2cap_sig_rx(uint16_t conn_handle, struct os_mbuf **om)
 
     rx_cb = ble_l2cap_sig_dispatch_get(hdr.op);
     if (rx_cb == NULL) {
-        ble_l2cap_sig_reject_tx(conn_handle, hdr.identifier,
-                                BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD);
-        rc = BLE_HS_ENOTSUP;
+        ble_hs_lock();
+        rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
+                                             &conn, &chan);
+        if (rc == 0) {
+            ble_l2cap_sig_reject_tx(conn, chan, hdr.identifier,
+                                    BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD,
+                                    NULL, 0);
+            rc = BLE_HS_L2C_ERR(BLE_L2CAP_SIG_ERR_CMD_NOT_UNDERSTOOD);
+        } else {
+            rc = BLE_HS_ENOTCONN;
+        }
+        ble_hs_unlock();
     } else {
         rc = rx_cb(conn_handle, &hdr, om);
     }
diff --git a/net/nimble/host/src/ble_l2cap_sig_cmd.c b/net/nimble/host/src/ble_l2cap_sig_cmd.c
index 7e121802..a93833c6 100644
--- a/net/nimble/host/src/ble_l2cap_sig_cmd.c
+++ b/net/nimble/host/src/ble_l2cap_sig_cmd.c
@@ -17,6 +17,7 @@
  * under the License.
  */
 
+#include <string.h>
 #include "ble_hs_priv.h"
 
 int
@@ -52,28 +53,6 @@ ble_l2cap_sig_init_cmd(uint8_t op, uint8_t id, uint8_t payload_len,
     return 0;
 }
 
-static int
-ble_l2cap_sig_tx(uint16_t conn_handle, struct os_mbuf *txom)
-{
-    struct ble_l2cap_chan *chan;
-    struct ble_hs_conn *conn;
-    int rc;
-
-    STATS_INC(ble_l2cap_stats, sig_tx);
-
-    ble_hs_lock();
-
-    rc = ble_hs_misc_conn_chan_find_reqd(conn_handle, BLE_L2CAP_CID_SIG,
-                                         &conn, &chan);
-    if (rc == 0) {
-        rc = ble_l2cap_tx(conn, chan, txom);
-    }
-
-    ble_hs_unlock();
-
-    return rc;
-}
-
 static void
 ble_l2cap_sig_hdr_swap(struct ble_l2cap_sig_hdr *dst,
                        struct ble_l2cap_sig_hdr *src)
@@ -107,31 +86,61 @@ ble_l2cap_sig_reject_swap(struct ble_l2cap_sig_reject *dst,
 
 static void
 ble_l2cap_sig_reject_write(void *payload, uint16_t len,
-                           struct ble_l2cap_sig_reject *src)
+                           struct ble_l2cap_sig_reject *src,
+                           void *data, int data_len)
 {
-    BLE_HS_DBG_ASSERT(len >= BLE_L2CAP_SIG_REJECT_MIN_SZ);
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(len >= BLE_L2CAP_SIG_REJECT_MIN_SZ + data_len);
+
     ble_l2cap_sig_reject_swap(payload, src);
+
+    u8ptr = payload;
+    u8ptr += BLE_L2CAP_SIG_REJECT_MIN_SZ;
+    memcpy(u8ptr, data, data_len);
 }
 
 int
-ble_l2cap_sig_reject_tx(uint16_t conn_handle, uint8_t id, uint16_t reason)
+ble_l2cap_sig_reject_tx(struct ble_hs_conn *conn, struct ble_l2cap_chan *chan,
+                        uint8_t id, uint16_t reason,
+                        void *data, int data_len)
 {
-    /* XXX: Add support for optional data field. */
-
     struct ble_l2cap_sig_reject cmd;
     struct os_mbuf *txom;
     void *payload_buf;
     int rc;
 
     rc = ble_l2cap_sig_init_cmd(BLE_L2CAP_SIG_OP_REJECT, id,
-                                BLE_L2CAP_SIG_REJECT_MIN_SZ, &txom,
+                                BLE_L2CAP_SIG_REJECT_MIN_SZ + data_len, &txom,
                                 &payload_buf);
 
     cmd.reason = reason;
-    ble_l2cap_sig_reject_write(payload_buf, BLE_L2CAP_SIG_REJECT_MIN_SZ, &cmd);
+    ble_l2cap_sig_reject_write(payload_buf, txom->om_len, &cmd,
+                               data, data_len);
 
     STATS_INC(ble_l2cap_stats, sig_rx);
-    rc = ble_l2cap_sig_tx(conn_handle, txom);
+    rc = ble_l2cap_tx(conn, chan, txom);
+    return rc;
+}
+
+int
+ble_l2cap_sig_reject_invalid_cid_tx(struct ble_hs_conn *conn,
+                                    struct ble_l2cap_chan *chan, uint8_t id,
+                                    uint16_t src_cid, uint16_t dst_cid)
+{
+    int rc;
+
+    struct {
+        uint16_t local_cid;
+        uint16_t remote_cid;
+    } data = {
+        .local_cid = dst_cid,
+        .remote_cid = src_cid,
+    };
+
+    rc = ble_l2cap_sig_reject_tx(conn, chan, id,
+                                 BLE_L2CAP_SIG_ERR_INVALID_CID,
+                                 &data, sizeof data);
     return rc;
 }
 
@@ -162,7 +171,8 @@ ble_l2cap_sig_update_req_write(void *payload, int len,
 }
 
 int
-ble_l2cap_sig_update_req_tx(uint16_t conn_handle, uint8_t id,
+ble_l2cap_sig_update_req_tx(struct ble_hs_conn *conn,
+                            struct ble_l2cap_chan *chan, uint8_t id,
                             struct ble_l2cap_sig_update_req *req)
 {
     struct os_mbuf *txom;
@@ -179,7 +189,7 @@ ble_l2cap_sig_update_req_tx(uint16_t conn_handle, uint8_t id,
     ble_l2cap_sig_update_req_write(payload_buf, BLE_L2CAP_SIG_UPDATE_REQ_SZ,
                                    req);
 
-    rc = ble_l2cap_sig_tx(conn_handle, txom);
+    rc = ble_l2cap_tx(conn, chan, txom);
     if (rc != 0) {
         return rc;
     }
@@ -211,7 +221,9 @@ ble_l2cap_sig_update_rsp_write(void *payload, int len,
 }
 
 int
-ble_l2cap_sig_update_rsp_tx(uint16_t conn_handle, uint8_t id, uint16_t result)
+ble_l2cap_sig_update_rsp_tx(struct ble_hs_conn *conn,
+                            struct ble_l2cap_chan *chan, uint8_t id,
+                            uint16_t result)
 {
     struct ble_l2cap_sig_update_rsp rsp;
     struct os_mbuf *txom;
@@ -229,7 +241,7 @@ ble_l2cap_sig_update_rsp_tx(uint16_t conn_handle, uint8_t id, uint16_t result)
     ble_l2cap_sig_update_rsp_write(payload_buf, BLE_L2CAP_SIG_UPDATE_RSP_SZ,
                                    &rsp);
 
-    rc = ble_l2cap_sig_tx(conn_handle, txom);
+    rc = ble_l2cap_tx(conn, chan, txom);
     if (rc != 0) {
         return rc;
     }
diff --git a/net/nimble/host/src/ble_l2cap_sig_priv.h b/net/nimble/host/src/ble_l2cap_sig_priv.h
index da6d3b40..82eb733e 100644
--- a/net/nimble/host/src/ble_l2cap_sig_priv.h
+++ b/net/nimble/host/src/ble_l2cap_sig_priv.h
@@ -56,20 +56,30 @@ void ble_l2cap_sig_hdr_parse(void *payload, uint16_t len,
                              struct ble_l2cap_sig_hdr *hdr);
 void ble_l2cap_sig_hdr_write(void *payload, uint16_t len,
                              struct ble_l2cap_sig_hdr *hdr);
-int ble_l2cap_sig_reject_tx(uint16_t conn_handle, uint8_t id, uint16_t reason);
+int ble_l2cap_sig_reject_tx(struct ble_hs_conn *conn,
+                            struct ble_l2cap_chan *chan,
+                            uint8_t id, uint16_t reason,
+                            void *data, int data_len);
 void ble_l2cap_sig_update_req_parse(void *payload, int len,
                                     struct ble_l2cap_sig_update_req *req);
 void ble_l2cap_sig_update_req_write(void *payload, int len,
                                     struct ble_l2cap_sig_update_req *src);
-int ble_l2cap_sig_update_req_tx(uint16_t conn_handle, uint8_t id,
+int ble_l2cap_sig_update_req_tx(struct ble_hs_conn *conn,
+                                struct ble_l2cap_chan *chan, uint8_t id,
                                 struct ble_l2cap_sig_update_req *req);
 void ble_l2cap_sig_update_rsp_parse(void *payload, int len,
                                     struct ble_l2cap_sig_update_rsp *cmd);
 void ble_l2cap_sig_update_rsp_write(void *payload, int len,
                                     struct ble_l2cap_sig_update_rsp *src);
-int ble_l2cap_sig_update_rsp_tx(uint16_t conn_handle, uint8_t id,
+int ble_l2cap_sig_update_rsp_tx(struct ble_hs_conn *conn,
+                                struct ble_l2cap_chan *chan, uint8_t id,
                                 uint16_t result);
 
+int ble_l2cap_sig_reject_invalid_cid_tx(struct ble_hs_conn *conn,
+                                        struct ble_l2cap_chan *chan,
+                                        uint8_t id,
+                                        uint16_t src_cid, uint16_t dst_cid);
+
 void ble_l2cap_sig_heartbeat(void);
 struct ble_l2cap_chan *ble_l2cap_sig_create_chan(void);
 int ble_l2cap_sig_init(void);