Bluetooth: Add server socket support for LE connection

Add support for LE server sockets.

Signed-off-by: Ville Tervo <ville.tervo@nokia.com>
Acked-by: Marcel Holtmann <marcel@holtmann.org>
Signed-off-by: Gustavo F. Padovan <padovan@profusion.mobi>

3 files changed, 104 insertions, 7 deletions

diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 3155ad588076..74f04a27734d 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -2405,8 +2405,14 @@ static inline void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff
         hci_dev_lock(hdev);
 
         conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &ev->bdaddr);
-        if (!conn)
-                goto unlock;
+        if (!conn) {
+                conn = hci_conn_add(hdev, LE_LINK, &ev->bdaddr);
+                if (!conn) {
+                        BT_ERR("No memory for new connection");
+                        hci_dev_unlock(hdev);
+                        return;
+                }
+        }
 
         if (ev->status) {
                 hci_proto_connect_cfm(conn, ev->status);
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 123c1bfa0ce6..3079175065d4 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -181,8 +181,16 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct so
         l2cap_pi(sk)->conn = conn;
 
         if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) {
-                /* Alloc CID for connection-oriented socket */
-                l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
+                if (conn->hcon->type == LE_LINK) {
+                        /* LE connection */
+                        l2cap_pi(sk)->omtu = L2CAP_LE_DEFAULT_MTU;
+                        l2cap_pi(sk)->scid = L2CAP_CID_LE_DATA;
+                        l2cap_pi(sk)->dcid = L2CAP_CID_LE_DATA;
+                } else {
+                        /* Alloc CID for connection-oriented socket */
+                        l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
+                        l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU;
+                }
         } else if (sk->sk_type == SOCK_DGRAM) {
                 /* Connectionless socket */
                 l2cap_pi(sk)->scid = L2CAP_CID_CONN_LESS;
@@ -581,6 +589,82 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
         }
 }
 
+/* Find socket with cid and source bdaddr.
+ * Returns closest match, locked.
+ */
+static struct sock *l2cap_get_sock_by_scid(int state, __le16 cid, bdaddr_t *src)
+{
+        struct sock *s, *sk = NULL, *sk1 = NULL;
+        struct hlist_node *node;
+
+        read_lock(&l2cap_sk_list.lock);
+
+        sk_for_each(sk, node, &l2cap_sk_list.head) {
+                if (state && sk->sk_state != state)
+                        continue;
+
+                if (l2cap_pi(sk)->scid == cid) {
+                        /* Exact match. */
+                        if (!bacmp(&bt_sk(sk)->src, src))
+                                break;
+
+                        /* Closest match */
+                        if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
+                                sk1 = sk;
+                }
+        }
+        s = node ? sk : sk1;
+        if (s)
+                bh_lock_sock(s);
+        read_unlock(&l2cap_sk_list.lock);
+
+        return s;
+}
+
+static void l2cap_le_conn_ready(struct l2cap_conn *conn)
+{
+        struct l2cap_chan_list *list = &conn->chan_list;
+        struct sock *parent, *uninitialized_var(sk);
+
+        BT_DBG("");
+
+        /* Check if we have socket listening on cid */
+        parent = l2cap_get_sock_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA,
+                                                        conn->src);
+        if (!parent)
+                return;
+
+        /* Check for backlog size */
+        if (sk_acceptq_is_full(parent)) {
+                BT_DBG("backlog full %d", parent->sk_ack_backlog);
+                goto clean;
+        }
+
+        sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC);
+        if (!sk)
+                goto clean;
+
+        write_lock_bh(&list->lock);
+
+        hci_conn_hold(conn->hcon);
+
+        l2cap_sock_init(sk, parent);
+        bacpy(&bt_sk(sk)->src, conn->src);
+        bacpy(&bt_sk(sk)->dst, conn->dst);
+
+        __l2cap_chan_add(conn, sk, parent);
+
+        l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
+
+        sk->sk_state = BT_CONNECTED;
+        parent->sk_data_ready(parent, 0);
+
+        write_unlock_bh(&list->lock);
+
+clean:
+        bh_unlock_sock(parent);
+}
+
 static void l2cap_conn_ready(struct l2cap_conn *conn)
 {
         struct l2cap_chan_list *l = &conn->chan_list;
@@ -588,6 +672,9 @@ static void l2cap_conn_ready(struct l2cap_conn *conn)
 
         BT_DBG("conn %p", conn);
 
+        if (!conn->hcon->out && conn->hcon->type == LE_LINK)
+                l2cap_le_conn_ready(conn);
+
         read_lock(&l->lock);
 
         for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
@@ -670,7 +757,8 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
         spin_lock_init(&conn->lock);
         rwlock_init(&conn->chan_list.lock);
 
-        setup_timer(&conn->info_timer, l2cap_info_timeout,
+        if (hcon->type != LE_LINK)
+                setup_timer(&conn->info_timer, l2cap_info_timeout,
                                                 (unsigned long) conn);
 
         conn->disc_reason = 0x13;
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index f45d361e84d1..a8d289373794 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -103,7 +103,7 @@ static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
         len = min_t(unsigned int, sizeof(la), alen);
         memcpy(&la, addr, len);
 
-        if (la.l2_cid)
+        if (la.l2_cid && la.l2_psm)
                 return -EINVAL;
 
         lock_sock(sk);
@@ -145,6 +145,9 @@ static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
                         l2cap_pi(sk)->sec_level = BT_SECURITY_SDP;
         }
 
+        if (la.l2_cid)
+                l2cap_pi(sk)->scid = la.l2_cid;
+
         write_unlock_bh(&l2cap_sk_list.lock);
 
 done:
@@ -266,7 +269,7 @@ static int l2cap_sock_listen(struct socket *sock, int backlog)
                 goto done;
         }
 
-        if (!l2cap_pi(sk)->psm) {
+        if (!l2cap_pi(sk)->psm && !l2cap_pi(sk)->dcid) {
                 bdaddr_t *src = &bt_sk(sk)->src;
                 u16 psm;