[Bluetooth] Retrieve L2CAP features mask on connection setup

The Bluetooth 1.2 specification introduced a specific features mask
value to interoperate with newer versions of the specification. So far
this piece of information was never needed, but future extensions will
rely on it. This patch adds a generic way to retrieve this information
only once per connection setup.

Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

1 files changed, 151 insertions, 82 deletions

diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index fde1606a4f31..896ae5a9c07a 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -258,7 +258,119 @@ static void l2cap_chan_del(struct sock *sk, int err)
                 sk->sk_state_change(sk);
 }
 
+static inline u8 l2cap_get_ident(struct l2cap_conn *conn)
+{
+        u8 id;
+
+        /* Get next available identificator.
+         *    1 - 128 are used by kernel.
+         *  129 - 199 are reserved.
+         *  200 - 254 are used by utilities like l2ping, etc.
+         */
+
+        spin_lock_bh(&conn->lock);
+
+        if (++conn->tx_ident > 128)
+                conn->tx_ident = 1;
+
+        id = conn->tx_ident;
+
+        spin_unlock_bh(&conn->lock);
+
+        return id;
+}
+
+static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
+{
+        struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
+
+        BT_DBG("code 0x%2.2x", code);
+
+        if (!skb)
+                return -ENOMEM;
+
+        return hci_send_acl(conn->hcon, skb, 0);
+}
+
 /* ---- L2CAP connections ---- */
+static void l2cap_conn_start(struct l2cap_conn *conn)
+{
+        struct l2cap_chan_list *l = &conn->chan_list;
+        struct sock *sk;
+
+        BT_DBG("conn %p", conn);
+
+        read_lock(&l->lock);
+
+        for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
+                bh_lock_sock(sk);
+
+                if (sk->sk_type != SOCK_SEQPACKET) {
+                        l2cap_sock_clear_timer(sk);
+                        sk->sk_state = BT_CONNECTED;
+                        sk->sk_state_change(sk);
+                } else if (sk->sk_state == BT_CONNECT) {
+                        struct l2cap_conn_req req;
+                        l2cap_pi(sk)->ident = l2cap_get_ident(conn);
+                        req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
+                        req.psm  = l2cap_pi(sk)->psm;
+                        l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
+                                        L2CAP_CONN_REQ, sizeof(req), &req);
+                }
+
+                bh_unlock_sock(sk);
+        }
+
+        read_unlock(&l->lock);
+}
+
+static void l2cap_conn_ready(struct l2cap_conn *conn)
+{
+        BT_DBG("conn %p", conn);
+
+        if (conn->chan_list.head || !hlist_empty(&l2cap_sk_list.head)) {
+                struct l2cap_info_req req;
+
+                req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK);
+
+                conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT;
+                conn->info_ident = l2cap_get_ident(conn);
+
+                mod_timer(&conn->info_timer,
+                        jiffies + msecs_to_jiffies(L2CAP_INFO_TIMEOUT));
+
+                l2cap_send_cmd(conn, conn->info_ident,
+                                        L2CAP_INFO_REQ, sizeof(req), &req);
+        }
+}
+
+/* Notify sockets that we cannot guaranty reliability anymore */
+static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
+{
+        struct l2cap_chan_list *l = &conn->chan_list;
+        struct sock *sk;
+
+        BT_DBG("conn %p", conn);
+
+        read_lock(&l->lock);
+
+        for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
+                if (l2cap_pi(sk)->link_mode & L2CAP_LM_RELIABLE)
+                        sk->sk_err = err;
+        }
+
+        read_unlock(&l->lock);
+}
+
+static void l2cap_info_timeout(unsigned long arg)
+{
+        struct l2cap_conn *conn = (void *) arg;
+
+        conn->info_ident = 0;
+
+        l2cap_conn_start(conn);
+}
+
 static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
 {
         struct l2cap_conn *conn = hcon->l2cap_data;
@@ -279,6 +391,12 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
         conn->src = &hcon->hdev->bdaddr;
         conn->dst = &hcon->dst;
 
+        conn->feat_mask = 0;
+
+        init_timer(&conn->info_timer);
+        conn->info_timer.function = l2cap_info_timeout;
+        conn->info_timer.data = (unsigned long) conn;
+
         spin_lock_init(&conn->lock);
         rwlock_init(&conn->chan_list.lock);
 
@@ -318,40 +436,6 @@ static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, stru
         write_unlock_bh(&l->lock);
 }
 
-static inline u8 l2cap_get_ident(struct l2cap_conn *conn)
-{
-        u8 id;
-
-        /* Get next available identificator.
-         *    1 - 128 are used by kernel.
-         *  129 - 199 are reserved.
-         *  200 - 254 are used by utilities like l2ping, etc.
-         */
-
-        spin_lock_bh(&conn->lock);
-
-        if (++conn->tx_ident > 128)
-                conn->tx_ident = 1;
-
-        id = conn->tx_ident;
-
-        spin_unlock_bh(&conn->lock);
-
-        return id;
-}
-
-static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
-{
-        struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
-
-        BT_DBG("code 0x%2.2x", code);
-
-        if (!skb)
-                return -ENOMEM;
-
-        return hci_send_acl(conn->hcon, skb, 0);
-}
-
 /* ---- Socket interface ---- */
 static struct sock *__l2cap_get_sock_by_addr(__le16 psm, bdaddr_t *src)
 {
@@ -529,7 +613,7 @@ static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int p
         INIT_LIST_HEAD(&bt_sk(sk)->accept_q);
 
         sk->sk_destruct = l2cap_sock_destruct;
-        sk->sk_sndtimeo = L2CAP_CONN_TIMEOUT;
+        sk->sk_sndtimeo = msecs_to_jiffies(L2CAP_CONN_TIMEOUT);
 
         sock_reset_flag(sk, SOCK_ZAPPED);
 
@@ -649,6 +733,11 @@ static int l2cap_do_connect(struct sock *sk)
         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
 
         if (hcon->state == BT_CONNECTED) {
+                if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)) {
+                        l2cap_conn_ready(conn);
+                        goto done;
+                }
+
                 if (sk->sk_type == SOCK_SEQPACKET) {
                         struct l2cap_conn_req req;
                         l2cap_pi(sk)->ident = l2cap_get_ident(conn);
@@ -1083,52 +1172,6 @@ static int l2cap_sock_release(struct socket *sock)
         return err;
 }
 
-static void l2cap_conn_ready(struct l2cap_conn *conn)
-{
-        struct l2cap_chan_list *l = &conn->chan_list;
-        struct sock *sk;
-
-        BT_DBG("conn %p", conn);
-
-        read_lock(&l->lock);
-
-        for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
-                bh_lock_sock(sk);
-
-                if (sk->sk_type != SOCK_SEQPACKET) {
-                        l2cap_sock_clear_timer(sk);
-                        sk->sk_state = BT_CONNECTED;
-                        sk->sk_state_change(sk);
-                } else if (sk->sk_state == BT_CONNECT) {
-                        struct l2cap_conn_req req;
-                        l2cap_pi(sk)->ident = l2cap_get_ident(conn);
-                        req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
-                        req.psm  = l2cap_pi(sk)->psm;
-                        l2cap_send_cmd(conn, l2cap_pi(sk)->ident, L2CAP_CONN_REQ, sizeof(req), &req);
-                }
-
-                bh_unlock_sock(sk);
-        }
-
-        read_unlock(&l->lock);
-}
-
-/* Notify sockets that we cannot guaranty reliability anymore */
-static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
-{
-        struct l2cap_chan_list *l = &conn->chan_list;
-        struct sock *sk;
-
-        BT_DBG("conn %p", conn);
-
-        read_lock(&l->lock);
-        for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
-                if (l2cap_pi(sk)->link_mode & L2CAP_LM_RELIABLE)
-                        sk->sk_err = err;
-        }
-        read_unlock(&l->lock);
-}
-
 static void l2cap_chan_ready(struct sock *sk)
 {
         struct sock *parent = bt_sk(sk)->parent;
@@ -1399,6 +1442,23 @@ static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 fla
         return ptr - data;
 }
 
+static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+{
+        struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data;
+
+        if (rej->reason != 0x0000)
+                return 0;
+
+        if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
+                                        cmd->ident == conn->info_ident) {
+                conn->info_ident = 0;
+                del_timer(&conn->info_timer);
+                l2cap_conn_start(conn);
+        }
+
+        return 0;
+}
+
 static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_chan_list *list = &conn->chan_list;
@@ -1739,6 +1799,15 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
 
         BT_DBG("type 0x%4.4x result 0x%2.2x", type, result);
 
+        conn->info_ident = 0;
+
+        del_timer(&conn->info_timer);
+
+        if (type == L2CAP_IT_FEAT_MASK)
+                conn->feat_mask = __le32_to_cpu(get_unaligned((__le32 *) rsp->data));
+
+        l2cap_conn_start(conn);
+
         return 0;
 }
 
@@ -1768,7 +1837,7 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
 
                 switch (cmd.code) {
                 case L2CAP_COMMAND_REJ:
-                        /* FIXME: We should process this */
+                        l2cap_command_rej(conn, &cmd, data);
                         break;
 
                 case L2CAP_CONN_REQ: