Bluetooth: Add basic LE L2CAP connect request receiving support

This patch adds the necessary boiler plate code to handle receiving
L2CAP connect requests over LE and respond to them with a proper connect
response.

Signed-off-by: Johan Hedberg <johan.hedberg@intel.com>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

1 files changed, 152 insertions, 0 deletions

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 5f9287fd86df..d6f518d7e61b 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -617,6 +617,29 @@ void l2cap_chan_del(struct l2cap_chan *chan, int err)
         return;
 }
 
+static void l2cap_chan_le_connect_reject(struct l2cap_chan *chan)
+{
+        struct l2cap_conn *conn = chan->conn;
+        struct l2cap_le_conn_rsp rsp;
+        u16 result;
+
+        if (test_bit(FLAG_DEFER_SETUP, &chan->flags))
+                result = L2CAP_CR_AUTHORIZATION;
+        else
+                result = L2CAP_CR_BAD_PSM;
+
+        l2cap_state_change(chan, BT_DISCONN);
+
+        rsp.dcid    = cpu_to_le16(chan->scid);
+        rsp.mtu     = cpu_to_le16(chan->imtu);
+        rsp.mps     = __constant_cpu_to_le16(L2CAP_LE_DEFAULT_MPS);
+        rsp.credits = __constant_cpu_to_le16(L2CAP_LE_MAX_CREDITS);
+        rsp.result  = cpu_to_le16(result);
+
+        l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp),
+                       &rsp);
+}
+
 static void l2cap_chan_connect_reject(struct l2cap_chan *chan)
 {
         struct l2cap_conn *conn = chan->conn;
@@ -663,6 +686,8 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
                 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) {
                         if (conn->hcon->type == ACL_LINK)
                                 l2cap_chan_connect_reject(chan);
+                        else if (conn->hcon->type == LE_LINK)
+                                l2cap_chan_le_connect_reject(chan);
                 }
 
                 l2cap_chan_del(chan, reason);
@@ -3641,6 +3666,23 @@ static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data,
         return ptr - data;
 }
 
+void __l2cap_le_connect_rsp_defer(struct l2cap_chan *chan)
+{
+        struct l2cap_le_conn_rsp rsp;
+        struct l2cap_conn *conn = chan->conn;
+
+        BT_DBG("chan %p", chan);
+
+        rsp.dcid    = cpu_to_le16(chan->scid);
+        rsp.mtu     = cpu_to_le16(chan->imtu);
+        rsp.mps     = __constant_cpu_to_le16(L2CAP_LE_DEFAULT_MPS);
+        rsp.credits = __constant_cpu_to_le16(L2CAP_LE_MAX_CREDITS);
+        rsp.result  = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
+
+        l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp),
+                       &rsp);
+}
+
 void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
 {
         struct l2cap_conn_rsp rsp;
@@ -5382,6 +5424,113 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
         return err;
 }
 
+static int l2cap_le_connect_req(struct l2cap_conn *conn,
+                                struct l2cap_cmd_hdr *cmd, u16 cmd_len,
+                                u8 *data)
+{
+        struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data;
+        struct l2cap_le_conn_rsp rsp;
+        struct l2cap_chan *chan, *pchan;
+        u16 dcid, scid, mtu, mps;
+        __le16 psm;
+        u8 result;
+
+        if (cmd_len != sizeof(*req))
+                return -EPROTO;
+
+        scid = __le16_to_cpu(req->scid);
+        mtu  = __le16_to_cpu(req->mtu);
+        mps  = __le16_to_cpu(req->mps);
+        psm  = req->psm;
+        dcid = 0;
+
+        if (mtu < 23 || mps < 23)
+                return -EPROTO;
+
+        BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm),
+               scid, mtu, mps);
+
+        /* Check if we have socket listening on psm */
+        pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
+                                         &conn->hcon->dst, LE_LINK);
+        if (!pchan) {
+                result = L2CAP_CR_BAD_PSM;
+                chan = NULL;
+                goto response;
+        }
+
+        mutex_lock(&conn->chan_lock);
+        l2cap_chan_lock(pchan);
+
+        if (!smp_sufficient_security(conn->hcon, pchan->sec_level)) {
+                result = L2CAP_CR_AUTHENTICATION;
+                chan = NULL;
+                goto response_unlock;
+        }
+
+        /* Check if we already have channel with that dcid */
+        if (__l2cap_get_chan_by_dcid(conn, scid)) {
+                result = L2CAP_CR_NO_MEM;
+                chan = NULL;
+                goto response_unlock;
+        }
+
+        chan = pchan->ops->new_connection(pchan);
+        if (!chan) {
+                result = L2CAP_CR_NO_MEM;
+                goto response_unlock;
+        }
+
+        bacpy(&chan->src, &conn->hcon->src);
+        bacpy(&chan->dst, &conn->hcon->dst);
+        chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
+        chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
+        chan->psm  = psm;
+        chan->dcid = scid;
+        chan->omtu = mtu;
+        chan->remote_mps = mps;
+
+        __l2cap_chan_add(conn, chan);
+        dcid = chan->scid;
+
+        __set_chan_timer(chan, chan->ops->get_sndtimeo(chan));
+
+        chan->ident = cmd->ident;
+
+        if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
+                l2cap_state_change(chan, BT_CONNECT2);
+                result = L2CAP_CR_PEND;
+                chan->ops->defer(chan);
+        } else {
+                l2cap_chan_ready(chan);
+                result = L2CAP_CR_SUCCESS;
+        }
+
+response_unlock:
+        l2cap_chan_unlock(pchan);
+        mutex_unlock(&conn->chan_lock);
+
+        if (result == L2CAP_CR_PEND)
+                return 0;
+
+response:
+        if (chan) {
+                rsp.mtu = cpu_to_le16(chan->imtu);
+                rsp.mps = __constant_cpu_to_le16(L2CAP_LE_DEFAULT_MPS);
+        } else {
+                rsp.mtu = 0;
+                rsp.mps = 0;
+        }
+
+        rsp.dcid    = cpu_to_le16(dcid);
+        rsp.credits = __constant_cpu_to_le16(L2CAP_LE_MAX_CREDITS);
+        rsp.result  = cpu_to_le16(result);
+
+        l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp);
+
+        return 0;
+}
+
 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
                                    struct l2cap_cmd_hdr *cmd, u16 cmd_len,
                                    u8 *data)
@@ -5400,6 +5549,9 @@ static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
                 l2cap_le_connect_rsp(conn, cmd, cmd_len, data);
                 return 0;
 
+        case L2CAP_LE_CONN_REQ:
+                return l2cap_le_connect_req(conn, cmd, cmd_len, data);
+
         default:
                 BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code);
                 return -EINVAL;