Bluetooth: Disconnect early if mode is not supported

When mode is mandatory we shall not send connect request and report this
to the userspace as well.

Signed-off-by: Gustavo F. Padovan <padovan@profusion.mobi>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 7c695bf..f8bae54 100644 (file)
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -287,6 +287,11 @@ struct l2cap_conn {
        struct l2cap_chan_list chan_list;
 };
 
+struct sock_del_list {
+       struct sock *sk;
+       struct list_head list;
+};
+
 #define L2CAP_INFO_CL_MTU_REQ_SENT     0x01
 #define L2CAP_INFO_FEAT_MASK_REQ_SENT  0x04
 #define L2CAP_INFO_FEAT_MASK_REQ_DONE  0x08

diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 2fb45c4..6a33d26 100644 (file)
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -456,6 +456,22 @@ static void l2cap_do_start(struct sock *sk)
        }
 }
 
+static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
+{
+       u32 local_feat_mask = l2cap_feat_mask;
+       if (enable_ertm)
+               local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
+
+       switch (mode) {
+       case L2CAP_MODE_ERTM:
+               return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
+       case L2CAP_MODE_STREAMING:
+               return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
+       default:
+               return 0x00;
+       }
+}
+
 static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk, int err)
 {
        struct l2cap_disconn_req req;
@@ -484,10 +500,13 @@ static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk, int
 static void l2cap_conn_start(struct l2cap_conn *conn)
 {
        struct l2cap_chan_list *l = &conn->chan_list;
+       struct sock_del_list del, *tmp1, *tmp2;
        struct sock *sk;
 
        BT_DBG("conn %p", conn);
 
+       INIT_LIST_HEAD(&del.list);
+
        read_lock(&l->lock);
 
        for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
@@ -503,6 +522,19 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
                        if (l2cap_check_security(sk) &&
                                        __l2cap_no_conn_pending(sk)) {
                                struct l2cap_conn_req req;
+
+                               if (!l2cap_mode_supported(l2cap_pi(sk)->mode,
+                                               conn->feat_mask)
+                                               && l2cap_pi(sk)->conf_state &
+                                               L2CAP_CONF_STATE2_DEVICE) {
+                                       tmp1 = kzalloc(sizeof(struct srej_list),
+                                                       GFP_ATOMIC);
+                                       tmp1->sk = sk;
+                                       list_add_tail(&tmp1->list, &del.list);
+                                       bh_unlock_sock(sk);
+                                       continue;
+                               }
+
                                req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
                                req.psm  = l2cap_pi(sk)->psm;
 
@@ -542,6 +574,14 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
        }
 
        read_unlock(&l->lock);
+
+       list_for_each_entry_safe(tmp1, tmp2, &del.list, list) {
+               bh_lock_sock(tmp1->sk);
+               __l2cap_sock_close(tmp1->sk, ECONNRESET);
+               bh_unlock_sock(tmp1->sk);
+               list_del(&tmp1->list);
+               kfree(tmp1);
+       }
 }
 
 static void l2cap_conn_ready(struct l2cap_conn *conn)
@@ -2429,22 +2469,6 @@ static inline void l2cap_ertm_init(struct sock *sk)
        INIT_WORK(&l2cap_pi(sk)->busy_work, l2cap_busy_work);
 }
 
-static int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
-{
-       u32 local_feat_mask = l2cap_feat_mask;
-       if (enable_ertm)
-               local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
-
-       switch (mode) {
-       case L2CAP_MODE_ERTM:
-               return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
-       case L2CAP_MODE_STREAMING:
-               return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
-       default:
-               return 0x00;
-       }
-}
-
 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
 {
        switch (mode) {