Merge branch 'master' of git://git.kernel.org/pub/scm/linux/kernel/git/linville/wireless-next-2.6 into for-davem

29 files changed, 907 insertions, 402 deletions

diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index ee3b3049d385..ed371684c133 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -43,19 +43,6 @@ config BT_L2CAP
           Say Y here to compile L2CAP support into the kernel or say M to
           compile it as module (l2cap).
 
-config BT_L2CAP_EXT_FEATURES
-        bool "L2CAP Extended Features support (EXPERIMENTAL)"
-        depends on BT_L2CAP && EXPERIMENTAL
-        help
-          This option enables the L2CAP Extended Features support. These
-          new features include the Enhanced Retransmission and Streaming
-          Modes, the Frame Check Sequence (FCS), and Segmentation and
-          Reassembly (SAR) for L2CAP packets. They are a required for the
-          new Alternate MAC/PHY and the Bluetooth Medical Profile.
-
-          You should say N unless you know what you are doing. Note that
-          this is in an experimental state yet.
-
 config BT_SCO
         tristate "SCO links support"
         depends on BT
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 800b6b9fbbae..0b1e460fe440 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -1,6 +1,6 @@
 /*
    BlueZ - Bluetooth protocol stack for Linux
-   Copyright (C) 2000-2001 Qualcomm Incorporated
+   Copyright (c) 2000-2001, 2010, Code Aurora Forum. All rights reserved.
 
    Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
 
@@ -155,6 +155,27 @@ void hci_setup_sync(struct hci_conn *conn, __u16 handle)
         hci_send_cmd(hdev, HCI_OP_SETUP_SYNC_CONN, sizeof(cp), &cp);
 }
 
+/* Device _must_ be locked */
+void hci_sco_setup(struct hci_conn *conn, __u8 status)
+{
+        struct hci_conn *sco = conn->link;
+
+        BT_DBG("%p", conn);
+
+        if (!sco)
+                return;
+
+        if (!status) {
+                if (lmp_esco_capable(conn->hdev))
+                        hci_setup_sync(sco, conn->handle);
+                else
+                        hci_add_sco(sco, conn->handle);
+        } else {
+                hci_proto_connect_cfm(sco, status);
+                hci_conn_del(sco);
+        }
+}
+
 static void hci_conn_timeout(unsigned long arg)
 {
         struct hci_conn *conn = (void *) arg;
@@ -385,10 +406,13 @@ struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst, __u8
                 acl->power_save = 1;
                 hci_conn_enter_active_mode(acl);
 
-                if (lmp_esco_capable(hdev))
-                        hci_setup_sync(sco, acl->handle);
-                else
-                        hci_add_sco(sco, acl->handle);
+                if (test_bit(HCI_CONN_MODE_CHANGE_PEND, &acl->pend)) {
+                        /* defer SCO setup until mode change completed */
+                        set_bit(HCI_CONN_SCO_SETUP_PEND, &acl->pend);
+                        return sco;
+                }
+
+                hci_sco_setup(acl, 0x00);
         }
 
         return sco;
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 2f768de87011..8303f1c9ef54 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -562,6 +562,7 @@ static int hci_dev_do_close(struct hci_dev *hdev)
         hci_dev_lock_bh(hdev);
         inquiry_cache_flush(hdev);
         hci_conn_hash_flush(hdev);
+        hci_blacklist_clear(hdev);
         hci_dev_unlock_bh(hdev);
 
         hci_notify(hdev, HCI_DEV_DOWN);
@@ -913,7 +914,7 @@ int hci_register_dev(struct hci_dev *hdev)
         skb_queue_head_init(&hdev->cmd_q);
         skb_queue_head_init(&hdev->raw_q);
 
-        for (i = 0; i < 3; i++)
+        for (i = 0; i < NUM_REASSEMBLY; i++)
                 hdev->reassembly[i] = NULL;
 
         init_waitqueue_head(&hdev->req_wait_q);
@@ -923,6 +924,8 @@ int hci_register_dev(struct hci_dev *hdev)
 
         hci_conn_hash_init(hdev);
 
+        INIT_LIST_HEAD(&hdev->blacklist.list);
+
         memset(&hdev->stat, 0, sizeof(struct hci_dev_stats));
 
         atomic_set(&hdev->promisc, 0);
@@ -970,7 +973,7 @@ int hci_unregister_dev(struct hci_dev *hdev)
 
         hci_dev_do_close(hdev);
 
-        for (i = 0; i < 3; i++)
+        for (i = 0; i < NUM_REASSEMBLY; i++)
                 kfree_skb(hdev->reassembly[i]);
 
         hci_notify(hdev, HCI_DEV_UNREG);
@@ -1030,89 +1033,170 @@ int hci_recv_frame(struct sk_buff *skb)
 }
 EXPORT_SYMBOL(hci_recv_frame);
 
-/* Receive packet type fragment */
-#define __reassembly(hdev, type)  ((hdev)->reassembly[(type) - 2])
-
-int hci_recv_fragment(struct hci_dev *hdev, int type, void *data, int count)
+static int hci_reassembly(struct hci_dev *hdev, int type, void *data,
+                          int count, __u8 index, gfp_t gfp_mask)
 {
-        if (type < HCI_ACLDATA_PKT || type > HCI_EVENT_PKT)
+        int len = 0;
+        int hlen = 0;
+        int remain = count;
+        struct sk_buff *skb;
+        struct bt_skb_cb *scb;
+
+        if ((type < HCI_ACLDATA_PKT || type > HCI_EVENT_PKT) ||
+                                index >= NUM_REASSEMBLY)
                 return -EILSEQ;
 
+        skb = hdev->reassembly[index];
+
+        if (!skb) {
+                switch (type) {
+                case HCI_ACLDATA_PKT:
+                        len = HCI_MAX_FRAME_SIZE;
+                        hlen = HCI_ACL_HDR_SIZE;
+                        break;
+                case HCI_EVENT_PKT:
+                        len = HCI_MAX_EVENT_SIZE;
+                        hlen = HCI_EVENT_HDR_SIZE;
+                        break;
+                case HCI_SCODATA_PKT:
+                        len = HCI_MAX_SCO_SIZE;
+                        hlen = HCI_SCO_HDR_SIZE;
+                        break;
+                }
+
+                skb = bt_skb_alloc(len, gfp_mask);
+                if (!skb)
+                        return -ENOMEM;
+
+                scb = (void *) skb->cb;
+                scb->expect = hlen;
+                scb->pkt_type = type;
+
+                skb->dev = (void *) hdev;
+                hdev->reassembly[index] = skb;
+        }
+
         while (count) {
-                struct sk_buff *skb = __reassembly(hdev, type);
-                struct { int expect; } *scb;
-                int len = 0;
+                scb = (void *) skb->cb;
+                len = min(scb->expect, (__u16)count);
 
-                if (!skb) {
-                        /* Start of the frame */
+                memcpy(skb_put(skb, len), data, len);
 
-                        switch (type) {
-                        case HCI_EVENT_PKT:
-                                if (count >= HCI_EVENT_HDR_SIZE) {
-                                        struct hci_event_hdr *h = data;
-                                        len = HCI_EVENT_HDR_SIZE + h->plen;
-                                } else
-                                        return -EILSEQ;
-                                break;
+                count -= len;
+                data += len;
+                scb->expect -= len;
+                remain = count;
 
-                        case HCI_ACLDATA_PKT:
-                                if (count >= HCI_ACL_HDR_SIZE) {
-                                        struct hci_acl_hdr *h = data;
-                                        len = HCI_ACL_HDR_SIZE + __le16_to_cpu(h->dlen);
-                                } else
-                                        return -EILSEQ;
-                                break;
+                switch (type) {
+                case HCI_EVENT_PKT:
+                        if (skb->len == HCI_EVENT_HDR_SIZE) {
+                                struct hci_event_hdr *h = hci_event_hdr(skb);
+                                scb->expect = h->plen;
+
+                                if (skb_tailroom(skb) < scb->expect) {
+                                        kfree_skb(skb);
+                                        hdev->reassembly[index] = NULL;
+                                        return -ENOMEM;
+                                }
+                        }
+                        break;
 
-                        case HCI_SCODATA_PKT:
-                                if (count >= HCI_SCO_HDR_SIZE) {
-                                        struct hci_sco_hdr *h = data;
-                                        len = HCI_SCO_HDR_SIZE + h->dlen;
-                                } else
-                                        return -EILSEQ;
-                                break;
+                case HCI_ACLDATA_PKT:
+                        if (skb->len  == HCI_ACL_HDR_SIZE) {
+                                struct hci_acl_hdr *h = hci_acl_hdr(skb);
+                                scb->expect = __le16_to_cpu(h->dlen);
+
+                                if (skb_tailroom(skb) < scb->expect) {
+                                        kfree_skb(skb);
+                                        hdev->reassembly[index] = NULL;
+                                        return -ENOMEM;
+                                }
                         }
+                        break;
 
-                        skb = bt_skb_alloc(len, GFP_ATOMIC);
-                        if (!skb) {
-                                BT_ERR("%s no memory for packet", hdev->name);
-                                return -ENOMEM;
+                case HCI_SCODATA_PKT:
+                        if (skb->len == HCI_SCO_HDR_SIZE) {
+                                struct hci_sco_hdr *h = hci_sco_hdr(skb);
+                                scb->expect = h->dlen;
+
+                                if (skb_tailroom(skb) < scb->expect) {
+                                        kfree_skb(skb);
+                                        hdev->reassembly[index] = NULL;
+                                        return -ENOMEM;
+                                }
                         }
+                        break;
+                }
+
+                if (scb->expect == 0) {
+                        /* Complete frame */
 
-                        skb->dev = (void *) hdev;
                         bt_cb(skb)->pkt_type = type;
+                        hci_recv_frame(skb);
 
-                        __reassembly(hdev, type) = skb;
+                        hdev->reassembly[index] = NULL;
+                        return remain;
+                }
+        }
 
-                        scb = (void *) skb->cb;
-                        scb->expect = len;
-                } else {
-                        /* Continuation */
+        return remain;
+}
 
-                        scb = (void *) skb->cb;
-                        len = scb->expect;
-                }
+int hci_recv_fragment(struct hci_dev *hdev, int type, void *data, int count)
+{
+        int rem = 0;
 
-                len = min(len, count);
+        if (type < HCI_ACLDATA_PKT || type > HCI_EVENT_PKT)
+                return -EILSEQ;
 
-                memcpy(skb_put(skb, len), data, len);
+        while (count) {
+                rem = hci_reassembly(hdev, type, data, count,
+                                                type - 1, GFP_ATOMIC);
+                if (rem < 0)
+                        return rem;
 
-                scb->expect -= len;
+                data += (count - rem);
+                count = rem;
+        };
 
-                if (scb->expect == 0) {
-                        /* Complete frame */
+        return rem;
+}
+EXPORT_SYMBOL(hci_recv_fragment);
 
-                        __reassembly(hdev, type) = NULL;
+#define STREAM_REASSEMBLY 0
 
-                        bt_cb(skb)->pkt_type = type;
-                        hci_recv_frame(skb);
-                }
+int hci_recv_stream_fragment(struct hci_dev *hdev, void *data, int count)
+{
+        int type;
+        int rem = 0;
 
-                count -= len; data += len;
-        }
+        while (count) {
+                struct sk_buff *skb = hdev->reassembly[STREAM_REASSEMBLY];
 
-        return 0;
+                if (!skb) {
+                        struct { char type; } *pkt;
+
+                        /* Start of the frame */
+                        pkt = data;
+                        type = pkt->type;
+
+                        data++;
+                        count--;
+                } else
+                        type = bt_cb(skb)->pkt_type;
+
+                rem = hci_reassembly(hdev, type, data,
+                                        count, STREAM_REASSEMBLY, GFP_ATOMIC);
+                if (rem < 0)
+                        return rem;
+
+                data += (count - rem);
+                count = rem;
+        };
+
+        return rem;
 }
-EXPORT_SYMBOL(hci_recv_fragment);
+EXPORT_SYMBOL(hci_recv_stream_fragment);
 
 /* ---- Interface to upper protocols ---- */
 
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 786b5de0bac4..bfef5bae0b3a 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1,6 +1,6 @@
 /*
    BlueZ - Bluetooth protocol stack for Linux
-   Copyright (C) 2000-2001 Qualcomm Incorporated
+   Copyright (c) 2000-2001, 2010, Code Aurora Forum. All rights reserved.
 
    Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
 
@@ -584,7 +584,7 @@ static inline void hci_cs_create_conn(struct hci_dev *hdev, __u8 status)
                                 conn->out = 1;
                                 conn->link_mode |= HCI_LM_MASTER;
                         } else
-                                BT_ERR("No memmory for new connection");
+                                BT_ERR("No memory for new connection");
                 }
         }
 
@@ -785,9 +785,13 @@ static void hci_cs_sniff_mode(struct hci_dev *hdev, __u8 status)
         hci_dev_lock(hdev);
 
         conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle));
-        if (conn)
+        if (conn) {
                 clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->pend);
 
+                if (test_and_clear_bit(HCI_CONN_SCO_SETUP_PEND, &conn->pend))
+                        hci_sco_setup(conn, status);
+        }
+
         hci_dev_unlock(hdev);
 }
 
@@ -808,9 +812,13 @@ static void hci_cs_exit_sniff_mode(struct hci_dev *hdev, __u8 status)
         hci_dev_lock(hdev);
 
         conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle));
-        if (conn)
+        if (conn) {
                 clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->pend);
 
+                if (test_and_clear_bit(HCI_CONN_SCO_SETUP_PEND, &conn->pend))
+                        hci_sco_setup(conn, status);
+        }
+
         hci_dev_unlock(hdev);
 }
 
@@ -915,20 +923,8 @@ static inline void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *s
         } else
                 conn->state = BT_CLOSED;
 
-        if (conn->type == ACL_LINK) {
-                struct hci_conn *sco = conn->link;
-                if (sco) {
-                        if (!ev->status) {
-                                if (lmp_esco_capable(hdev))
-                                        hci_setup_sync(sco, conn->handle);
-                                else
-                                        hci_add_sco(sco, conn->handle);
-                        } else {
-                                hci_proto_connect_cfm(sco, ev->status);
-                                hci_conn_del(sco);
-                        }
-                }
-        }
+        if (conn->type == ACL_LINK)
+                hci_sco_setup(conn, ev->status);
 
         if (ev->status) {
                 hci_proto_connect_cfm(conn, ev->status);
@@ -952,7 +948,7 @@ static inline void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *sk
 
         mask |= hci_proto_connect_ind(hdev, &ev->bdaddr, ev->link_type);
 
-        if (mask & HCI_LM_ACCEPT) {
+        if ((mask & HCI_LM_ACCEPT) && !hci_blacklist_lookup(hdev, &ev->bdaddr)) {
                 /* Connection accepted */
                 struct inquiry_entry *ie;
                 struct hci_conn *conn;
@@ -965,7 +961,7 @@ static inline void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *sk
                 conn = hci_conn_hash_lookup_ba(hdev, ev->link_type, &ev->bdaddr);
                 if (!conn) {
                         if (!(conn = hci_conn_add(hdev, ev->link_type, &ev->bdaddr))) {
-                                BT_ERR("No memmory for new connection");
+                                BT_ERR("No memory for new connection");
                                 hci_dev_unlock(hdev);
                                 return;
                         }
@@ -1481,6 +1477,9 @@ static inline void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb
                         else
                                 conn->power_save = 0;
                 }
+
+                if (test_and_clear_bit(HCI_CONN_SCO_SETUP_PEND, &conn->pend))
+                        hci_sco_setup(conn, ev->status);
         }
 
         hci_dev_unlock(hdev);
diff --git a/net/bluetooth/hci_sock.c b/net/bluetooth/hci_sock.c
index 38f08f6b86f6..4f170a595934 100644
--- a/net/bluetooth/hci_sock.c
+++ b/net/bluetooth/hci_sock.c
@@ -165,6 +165,86 @@ static int hci_sock_release(struct socket *sock)
         return 0;
 }
 
+struct bdaddr_list *hci_blacklist_lookup(struct hci_dev *hdev, bdaddr_t *bdaddr)
+{
+        struct list_head *p;
+        struct bdaddr_list *blacklist = &hdev->blacklist;
+
+        list_for_each(p, &blacklist->list) {
+                struct bdaddr_list *b;
+
+                b = list_entry(p, struct bdaddr_list, list);
+
+                if (bacmp(bdaddr, &b->bdaddr) == 0)
+                        return b;
+        }
+
+        return NULL;
+}
+
+static int hci_blacklist_add(struct hci_dev *hdev, void __user *arg)
+{
+        bdaddr_t bdaddr;
+        struct bdaddr_list *entry;
+
+        if (copy_from_user(&bdaddr, arg, sizeof(bdaddr)))
+                return -EFAULT;
+
+        if (bacmp(&bdaddr, BDADDR_ANY) == 0)
+                return -EBADF;
+
+        if (hci_blacklist_lookup(hdev, &bdaddr))
+                return -EEXIST;
+
+        entry = kzalloc(sizeof(struct bdaddr_list), GFP_KERNEL);
+        if (!entry)
+                return -ENOMEM;
+
+        bacpy(&entry->bdaddr, &bdaddr);
+
+        list_add(&entry->list, &hdev->blacklist.list);
+
+        return 0;
+}
+
+int hci_blacklist_clear(struct hci_dev *hdev)
+{
+        struct list_head *p, *n;
+        struct bdaddr_list *blacklist = &hdev->blacklist;
+
+        list_for_each_safe(p, n, &blacklist->list) {
+                struct bdaddr_list *b;
+
+                b = list_entry(p, struct bdaddr_list, list);
+
+                list_del(p);
+                kfree(b);
+        }
+
+        return 0;
+}
+
+static int hci_blacklist_del(struct hci_dev *hdev, void __user *arg)
+{
+        bdaddr_t bdaddr;
+        struct bdaddr_list *entry;
+
+        if (copy_from_user(&bdaddr, arg, sizeof(bdaddr)))
+                return -EFAULT;
+
+        if (bacmp(&bdaddr, BDADDR_ANY) == 0)
+                return hci_blacklist_clear(hdev);
+
+        entry = hci_blacklist_lookup(hdev, &bdaddr);
+        if (!entry)
+                return -ENOENT;
+
+        list_del(&entry->list);
+        kfree(entry);
+
+        return 0;
+}
+
 /* Ioctls that require bound socket */
 static inline int hci_sock_bound_ioctl(struct sock *sk, unsigned int cmd, unsigned long arg)
 {
@@ -194,6 +274,16 @@ static inline int hci_sock_bound_ioctl(struct sock *sk, unsigned int cmd, unsign
         case HCIGETAUTHINFO:
                 return hci_get_auth_info(hdev, (void __user *) arg);
 
+        case HCIBLOCKADDR:
+                if (!capable(CAP_NET_ADMIN))
+                        return -EACCES;
+                return hci_blacklist_add(hdev, (void __user *) arg);
+
+        case HCIUNBLOCKADDR:
+                if (!capable(CAP_NET_ADMIN))
+                        return -EACCES;
+                return hci_blacklist_del(hdev, (void __user *) arg);
+
         default:
                 if (hdev->ioctl)
                         return hdev->ioctl(hdev, cmd, arg);
diff --git a/net/bluetooth/hci_sysfs.c b/net/bluetooth/hci_sysfs.c
index 463ffa4fe042..ce44c47eeac1 100644
--- a/net/bluetooth/hci_sysfs.c
+++ b/net/bluetooth/hci_sysfs.c
@@ -436,6 +436,41 @@ static const struct file_operations inquiry_cache_fops = {
         .release        = single_release,
 };
 
+static int blacklist_show(struct seq_file *f, void *p)
+{
+        struct hci_dev *hdev = f->private;
+        struct bdaddr_list *blacklist = &hdev->blacklist;
+        struct list_head *l;
+
+        hci_dev_lock_bh(hdev);
+
+        list_for_each(l, &blacklist->list) {
+                struct bdaddr_list *b;
+                bdaddr_t bdaddr;
+
+                b = list_entry(l, struct bdaddr_list, list);
+
+                baswap(&bdaddr, &b->bdaddr);
+
+                seq_printf(f, "%s\n", batostr(&bdaddr));
+        }
+
+        hci_dev_unlock_bh(hdev);
+
+        return 0;
+}
+
+static int blacklist_open(struct inode *inode, struct file *file)
+{
+        return single_open(file, blacklist_show, inode->i_private);
+}
+
+static const struct file_operations blacklist_fops = {
+        .open           = blacklist_open,
+        .read           = seq_read,
+        .llseek         = seq_lseek,
+        .release        = single_release,
+};
 int hci_register_sysfs(struct hci_dev *hdev)
 {
         struct device *dev = &hdev->dev;
@@ -465,6 +500,9 @@ int hci_register_sysfs(struct hci_dev *hdev)
         debugfs_create_file("inquiry_cache", 0444, hdev->debugfs,
                                                 hdev, &inquiry_cache_fops);
 
+        debugfs_create_file("blacklist", 0444, hdev->debugfs,
+                                                hdev, &blacklist_fops);
+
         return 0;
 }
 
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index cf3c4073a8a6..9ba1e8eee37c 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -1,6 +1,8 @@
 /*
    BlueZ - Bluetooth protocol stack for Linux
    Copyright (C) 2000-2001 Qualcomm Incorporated
+   Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org>
+   Copyright (C) 2010 Google Inc.
 
    Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com>
 
@@ -53,15 +55,9 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 
-#define VERSION "2.14"
+#define VERSION "2.15"
 
-#ifdef CONFIG_BT_L2CAP_EXT_FEATURES
-static int enable_ertm = 1;
-#else
-static int enable_ertm = 0;
-#endif
-static int max_transmit = L2CAP_DEFAULT_MAX_TX;
-static int tx_window = L2CAP_DEFAULT_TX_WINDOW;
+static int disable_ertm = 0;
 
 static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
 static u8 l2cap_fixed_chan[8] = { 0x02, };
@@ -80,9 +76,12 @@ static void __l2cap_sock_close(struct sock *sk, int reason);
 static void l2cap_sock_close(struct sock *sk);
 static void l2cap_sock_kill(struct sock *sk);
 
+static int l2cap_build_conf_req(struct sock *sk, void *data);
 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
                                 u8 code, u8 ident, u16 dlen, void *data);
 
+static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb);
+
 /* ---- L2CAP timers ---- */
 static void l2cap_sock_timeout(unsigned long arg)
 {
@@ -278,6 +277,24 @@ static void l2cap_chan_del(struct sock *sk, int err)
                 parent->sk_data_ready(parent, 0);
         } else
                 sk->sk_state_change(sk);
+
+        skb_queue_purge(TX_QUEUE(sk));
+
+        if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
+                struct srej_list *l, *tmp;
+
+                del_timer(&l2cap_pi(sk)->retrans_timer);
+                del_timer(&l2cap_pi(sk)->monitor_timer);
+                del_timer(&l2cap_pi(sk)->ack_timer);
+
+                skb_queue_purge(SREJ_QUEUE(sk));
+                skb_queue_purge(BUSY_QUEUE(sk));
+
+                list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
+                        list_del(&l->list);
+                        kfree(l);
+                }
+        }
 }
 
 /* Service level security */
@@ -351,8 +368,12 @@ static inline void l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
         struct sk_buff *skb;
         struct l2cap_hdr *lh;
         struct l2cap_conn *conn = pi->conn;
+        struct sock *sk = (struct sock *)pi;
         int count, hlen = L2CAP_HDR_SIZE + 2;
 
+        if (sk->sk_state != BT_CONNECTED)
+                return;
+
         if (pi->fcs == L2CAP_FCS_CRC16)
                 hlen += 2;
 
@@ -440,24 +461,57 @@ static void l2cap_do_start(struct sock *sk)
         }
 }
 
-static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk)
+static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
+{
+        u32 local_feat_mask = l2cap_feat_mask;
+        if (!disable_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;
 
+        if (!conn)
+                return;
+
+        skb_queue_purge(TX_QUEUE(sk));
+
+        if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
+                del_timer(&l2cap_pi(sk)->retrans_timer);
+                del_timer(&l2cap_pi(sk)->monitor_timer);
+                del_timer(&l2cap_pi(sk)->ack_timer);
+        }
+
         req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
         req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
         l2cap_send_cmd(conn, l2cap_get_ident(conn),
                         L2CAP_DISCONN_REQ, sizeof(req), &req);
+
+        sk->sk_state = BT_DISCONN;
+        sk->sk_err = err;
 }
 
 /* ---- L2CAP connections ---- */
 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) {
@@ -470,20 +524,38 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
                 }
 
                 if (sk->sk_state == BT_CONNECT) {
-                        if (l2cap_check_security(sk) &&
-                                        __l2cap_no_conn_pending(sk)) {
-                                struct l2cap_conn_req req;
-                                req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
-                                req.psm  = l2cap_pi(sk)->psm;
+                        struct l2cap_conn_req req;
 
-                                l2cap_pi(sk)->ident = l2cap_get_ident(conn);
-                                l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
+                        if (!l2cap_check_security(sk) ||
+                                        !__l2cap_no_conn_pending(sk)) {
+                                bh_unlock_sock(sk);
+                                continue;
+                        }
 
-                                l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
-                                        L2CAP_CONN_REQ, sizeof(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 sock_del_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;
+
+                        l2cap_pi(sk)->ident = l2cap_get_ident(conn);
+                        l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND;
+
+                        l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
+                                L2CAP_CONN_REQ, sizeof(req), &req);
+
                 } else if (sk->sk_state == BT_CONNECT2) {
                         struct l2cap_conn_rsp rsp;
+                        char buf[128];
                         rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid);
                         rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid);
 
@@ -506,12 +578,31 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 
                         l2cap_send_cmd(conn, l2cap_pi(sk)->ident,
                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
+
+                        if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT ||
+                                        rsp.result != L2CAP_CR_SUCCESS) {
+                                bh_unlock_sock(sk);
+                                continue;
+                        }
+
+                        l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
+                        l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
+                                                l2cap_build_conf_req(sk, buf), buf);
+                        l2cap_pi(sk)->num_conf_req++;
                 }
 
                 bh_unlock_sock(sk);
         }
 
         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)
@@ -740,9 +831,8 @@ static void __l2cap_sock_close(struct sock *sk, int reason)
                                 sk->sk_type == SOCK_STREAM) {
                         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
 
-                        sk->sk_state = BT_DISCONN;
                         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
-                        l2cap_send_disconn_req(conn, sk);
+                        l2cap_send_disconn_req(conn, sk, reason);
                 } else
                         l2cap_chan_del(sk, reason);
                 break;
@@ -802,6 +892,7 @@ static void l2cap_sock_init(struct sock *sk, struct sock *parent)
 
                 pi->imtu = l2cap_pi(parent)->imtu;
                 pi->omtu = l2cap_pi(parent)->omtu;
+                pi->conf_state = l2cap_pi(parent)->conf_state;
                 pi->mode = l2cap_pi(parent)->mode;
                 pi->fcs  = l2cap_pi(parent)->fcs;
                 pi->max_tx = l2cap_pi(parent)->max_tx;
@@ -812,13 +903,15 @@ static void l2cap_sock_init(struct sock *sk, struct sock *parent)
         } else {
                 pi->imtu = L2CAP_DEFAULT_MTU;
                 pi->omtu = 0;
-                if (enable_ertm && sk->sk_type == SOCK_STREAM)
+                if (!disable_ertm && sk->sk_type == SOCK_STREAM) {
                         pi->mode = L2CAP_MODE_ERTM;
-                else
+                        pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
+                } else {
                         pi->mode = L2CAP_MODE_BASIC;
-                pi->max_tx = max_transmit;
+                }
+                pi->max_tx = L2CAP_DEFAULT_MAX_TX;
                 pi->fcs  = L2CAP_FCS_CRC16;
-                pi->tx_win = tx_window;
+                pi->tx_win = L2CAP_DEFAULT_TX_WINDOW;
                 pi->sec_level = BT_SECURITY_LOW;
                 pi->role_switch = 0;
                 pi->force_reliable = 0;
@@ -1067,7 +1160,7 @@ static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int al
                 break;
         case L2CAP_MODE_ERTM:
         case L2CAP_MODE_STREAMING:
-                if (enable_ertm)
+                if (!disable_ertm)
                         break;
                 /* fall through */
         default:
@@ -1084,6 +1177,7 @@ static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int al
 
         case BT_CONNECTED:
                 /* Already connected */
+                err = -EISCONN;
                 goto done;
 
         case BT_OPEN:
@@ -1132,7 +1226,7 @@ static int l2cap_sock_listen(struct socket *sock, int backlog)
                 break;
         case L2CAP_MODE_ERTM:
         case L2CAP_MODE_STREAMING:
-                if (enable_ertm)
+                if (!disable_ertm)
                         break;
                 /* fall through */
         default:
@@ -1285,9 +1379,11 @@ static void l2cap_monitor_timeout(unsigned long arg)
 {
         struct sock *sk = (void *) arg;
 
+        BT_DBG("sk %p", sk);
+
         bh_lock_sock(sk);
         if (l2cap_pi(sk)->retry_count >= l2cap_pi(sk)->remote_max_tx) {
-                l2cap_send_disconn_req(l2cap_pi(sk)->conn, sk);
+                l2cap_send_disconn_req(l2cap_pi(sk)->conn, sk, ECONNABORTED);
                 bh_unlock_sock(sk);
                 return;
         }
@@ -1303,6 +1399,8 @@ static void l2cap_retrans_timeout(unsigned long arg)
 {
         struct sock *sk = (void *) arg;
 
+        BT_DBG("sk %p", sk);
+
         bh_lock_sock(sk);
         l2cap_pi(sk)->retry_count = 1;
         __mod_monitor_timer();
@@ -1341,7 +1439,7 @@ static inline void l2cap_do_send(struct sock *sk, struct sk_buff *skb)
         hci_send_acl(pi->conn->hcon, skb, 0);
 }
 
-static int l2cap_streaming_send(struct sock *sk)
+static void l2cap_streaming_send(struct sock *sk)
 {
         struct sk_buff *skb, *tx_skb;
         struct l2cap_pinfo *pi = l2cap_pi(sk);
@@ -1371,7 +1469,6 @@ static int l2cap_streaming_send(struct sock *sk)
                 skb = skb_dequeue(TX_QUEUE(sk));
                 kfree_skb(skb);
         }
-        return 0;
 }
 
 static void l2cap_retransmit_one_frame(struct sock *sk, u8 tx_seq)
@@ -1395,15 +1492,22 @@ static void l2cap_retransmit_one_frame(struct sock *sk, u8 tx_seq)
 
         if (pi->remote_max_tx &&
                         bt_cb(skb)->retries == pi->remote_max_tx) {
-                l2cap_send_disconn_req(pi->conn, sk);
+                l2cap_send_disconn_req(pi->conn, sk, ECONNABORTED);
                 return;
         }
 
         tx_skb = skb_clone(skb, GFP_ATOMIC);
         bt_cb(skb)->retries++;
         control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
+
+        if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
+                control |= L2CAP_CTRL_FINAL;
+                pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
+        }
+
         control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
                         | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
+
         put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
 
         if (pi->fcs == L2CAP_FCS_CRC16) {
@@ -1421,15 +1525,14 @@ static int l2cap_ertm_send(struct sock *sk)
         u16 control, fcs;
         int nsent = 0;
 
-        if (pi->conn_state & L2CAP_CONN_WAIT_F)
-                return 0;
+        if (sk->sk_state != BT_CONNECTED)
+                return -ENOTCONN;
 
-        while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk)) &&
-                        !(pi->conn_state & L2CAP_CONN_REMOTE_BUSY)) {
+        while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) {
 
                 if (pi->remote_max_tx &&
                                 bt_cb(skb)->retries == pi->remote_max_tx) {
-                        l2cap_send_disconn_req(pi->conn, sk);
+                        l2cap_send_disconn_req(pi->conn, sk, ECONNABORTED);
                         break;
                 }
 
@@ -1438,6 +1541,8 @@ static int l2cap_ertm_send(struct sock *sk)
                 bt_cb(skb)->retries++;
 
                 control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
+                control &= L2CAP_CTRL_SAR;
+
                 if (pi->conn_state & L2CAP_CONN_SEND_FBIT) {
                         control |= L2CAP_CTRL_FINAL;
                         pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
@@ -1478,16 +1583,11 @@ static int l2cap_retransmit_frames(struct sock *sk)
         struct l2cap_pinfo *pi = l2cap_pi(sk);
         int ret;
 
-        spin_lock_bh(&pi->send_lock);
-
         if (!skb_queue_empty(TX_QUEUE(sk)))
                 sk->sk_send_head = TX_QUEUE(sk)->next;
 
         pi->next_tx_seq = pi->expected_ack_seq;
         ret = l2cap_ertm_send(sk);
-
-        spin_unlock_bh(&pi->send_lock);
-
         return ret;
 }
 
@@ -1495,7 +1595,6 @@ static void l2cap_send_ack(struct l2cap_pinfo *pi)
 {
         struct sock *sk = (struct sock *)pi;
         u16 control = 0;
-        int nframes;
 
         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
 
@@ -1506,11 +1605,7 @@ static void l2cap_send_ack(struct l2cap_pinfo *pi)
                 return;
         }
 
-        spin_lock_bh(&pi->send_lock);
-        nframes = l2cap_ertm_send(sk);
-        spin_unlock_bh(&pi->send_lock);
-
-        if (nframes > 0)
+        if (l2cap_ertm_send(sk) > 0)
                 return;
 
         control |= L2CAP_SUPER_RCV_READY;
@@ -1705,10 +1800,8 @@ static inline int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, siz
                 size += buflen;
         }
         skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
-        spin_lock_bh(&pi->send_lock);
         if (sk->sk_send_head == NULL)
                 sk->sk_send_head = sar_queue.next;
-        spin_unlock_bh(&pi->send_lock);
 
         return size;
 }
@@ -1753,7 +1846,7 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
         case L2CAP_MODE_BASIC:
                 /* Check outgoing MTU */
                 if (len > pi->omtu) {
-                        err = -EINVAL;
+                        err = -EMSGSIZE;
                         goto done;
                 }
 
@@ -1780,14 +1873,9 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
                         }
                         __skb_queue_tail(TX_QUEUE(sk), skb);
 
-                        if (pi->mode == L2CAP_MODE_ERTM)
-                                spin_lock_bh(&pi->send_lock);
-
                         if (sk->sk_send_head == NULL)
                                 sk->sk_send_head = skb;
 
-                        if (pi->mode == L2CAP_MODE_ERTM)
-                                spin_unlock_bh(&pi->send_lock);
                 } else {
                 /* Segment SDU into multiples PDUs */
                         err = l2cap_sar_segment_sdu(sk, msg, len);
@@ -1796,11 +1884,14 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
                 }
 
                 if (pi->mode == L2CAP_MODE_STREAMING) {
-                        err = l2cap_streaming_send(sk);
+                        l2cap_streaming_send(sk);
                 } else {
-                        spin_lock_bh(&pi->send_lock);
+                        if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY &&
+                                        pi->conn_state && L2CAP_CONN_WAIT_F) {
+                                err = len;
+                                break;
+                        }
                         err = l2cap_ertm_send(sk);
-                        spin_unlock_bh(&pi->send_lock);
                 }
 
                 if (err >= 0)
@@ -1809,7 +1900,7 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
 
         default:
                 BT_DBG("bad state %1.1x", pi->mode);
-                err = -EINVAL;
+                err = -EBADFD;
         }
 
 done:
@@ -1825,6 +1916,8 @@ static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock, struct ms
 
         if (sk->sk_state == BT_CONNECT2 && bt_sk(sk)->defer_setup) {
                 struct l2cap_conn_rsp rsp;
+                struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+                u8 buf[128];
 
                 sk->sk_state = BT_CONFIG;
 
@@ -1835,6 +1928,16 @@ static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock, struct ms
                 l2cap_send_cmd(l2cap_pi(sk)->conn, l2cap_pi(sk)->ident,
                                         L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 
+                if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT) {
+                        release_sock(sk);
+                        return 0;
+                }
+
+                l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
+                l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
+                                l2cap_build_conf_req(sk, buf), buf);
+                l2cap_pi(sk)->num_conf_req++;
+
                 release_sock(sk);
                 return 0;
         }
@@ -1871,13 +1974,19 @@ static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __us
                         break;
                 }
 
+                if (opts.txwin_size > L2CAP_DEFAULT_TX_WINDOW) {
+                        err = -EINVAL;
+                        break;
+                }
+
                 l2cap_pi(sk)->mode = opts.mode;
                 switch (l2cap_pi(sk)->mode) {
                 case L2CAP_MODE_BASIC:
+                        l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_STATE2_DEVICE;
                         break;
                 case L2CAP_MODE_ERTM:
                 case L2CAP_MODE_STREAMING:
-                        if (enable_ertm)
+                        if (!disable_ertm)
                                 break;
                         /* fall through */
                 default:
@@ -2145,6 +2254,10 @@ static int l2cap_sock_shutdown(struct socket *sock, int how)
                         err = bt_sock_wait_state(sk, BT_CLOSED,
                                                         sk->sk_lingertime);
         }
+
+        if (!err && sk->sk_err)
+                err = -sk->sk_err;
+
         release_sock(sk);
         return err;
 }
@@ -2365,25 +2478,10 @@ static inline void l2cap_ertm_init(struct sock *sk)
 
         __skb_queue_head_init(SREJ_QUEUE(sk));
         __skb_queue_head_init(BUSY_QUEUE(sk));
-        spin_lock_init(&l2cap_pi(sk)->send_lock);
 
         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;
-        }
+        sk->sk_backlog_rcv = l2cap_ertm_data_rcv;
 }
 
 static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
@@ -2414,10 +2512,10 @@ static int l2cap_build_conf_req(struct sock *sk, void *data)
         switch (pi->mode) {
         case L2CAP_MODE_STREAMING:
         case L2CAP_MODE_ERTM:
-                pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
-                if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
-                        l2cap_send_disconn_req(pi->conn, sk);
-                break;
+                if (pi->conf_state & L2CAP_CONF_STATE2_DEVICE)
+                        break;
+
+                /* fall through */
         default:
                 pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
                 break;
@@ -2428,6 +2526,14 @@ done:
         case L2CAP_MODE_BASIC:
                 if (pi->imtu != L2CAP_DEFAULT_MTU)
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
+
+                rfc.mode            = L2CAP_MODE_BASIC;
+                rfc.txwin_size      = 0;
+                rfc.max_transmit    = 0;
+                rfc.retrans_timeout = 0;
+                rfc.monitor_timeout = 0;
+                rfc.max_pdu_size    = 0;
+
                 break;
 
         case L2CAP_MODE_ERTM:
@@ -2440,9 +2546,6 @@ done:
                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
                         rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
 
-                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
-                                        sizeof(rfc), (unsigned long) &rfc);
-
                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
                         break;
 
@@ -2463,9 +2566,6 @@ done:
                 if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10)
                         rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10);
 
-                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
-                                        sizeof(rfc), (unsigned long) &rfc);
-
                 if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
                         break;
 
@@ -2477,6 +2577,9 @@ done:
                 break;
         }
 
+        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
+                                                (unsigned long) &rfc);
+
         /* FIXME: Need actual value of the flush timeout */
         //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
         //   l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to);
@@ -2541,18 +2644,21 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data)
                 }
         }
 
-        if (pi->num_conf_rsp || pi->num_conf_req)
+        if (pi->num_conf_rsp || pi->num_conf_req > 1)
                 goto done;
 
         switch (pi->mode) {
         case L2CAP_MODE_STREAMING:
         case L2CAP_MODE_ERTM:
-                pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
-                if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
+                if (!(pi->conf_state & L2CAP_CONF_STATE2_DEVICE)) {
+                        pi->mode = l2cap_select_mode(rfc.mode,
+                                        pi->conn->feat_mask);
+                        break;
+                }
+
+                if (pi->mode != rfc.mode)
                         return -ECONNREFUSED;
-                break;
-        default:
-                pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
+
                 break;
         }
 
@@ -2675,7 +2781,6 @@ static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data,
                                                         rfc.mode != pi->mode)
                                 return -ECONNREFUSED;
 
-                        pi->mode = rfc.mode;
                         pi->fcs = 0;
 
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
@@ -2684,6 +2789,11 @@ static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data,
                 }
         }
 
+        if (pi->mode == L2CAP_MODE_BASIC && pi->mode != rfc.mode)
+                return -ECONNREFUSED;
+
+        pi->mode = rfc.mode;
+
         if (*result == L2CAP_CONF_SUCCESS) {
                 switch (rfc.mode) {
                 case L2CAP_MODE_ERTM:
@@ -2778,7 +2888,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
         struct l2cap_chan_list *list = &conn->chan_list;
         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
         struct l2cap_conn_rsp rsp;
-        struct sock *sk, *parent;
+        struct sock *parent, *uninitialized_var(sk);
         int result, status = L2CAP_CS_NO_INFO;
 
         u16 dcid = 0, scid = __le16_to_cpu(req->scid);
@@ -2887,6 +2997,15 @@ sendresp:
                                         L2CAP_INFO_REQ, sizeof(info), &info);
         }
 
+        if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT) &&
+                                result == L2CAP_CR_SUCCESS) {
+                u8 buf[128];
+                l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
+                l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
+                                        l2cap_build_conf_req(sk, buf), buf);
+                l2cap_pi(sk)->num_conf_req++;
+        }
+
         return 0;
 }
 
@@ -2907,11 +3026,11 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
         if (scid) {
                 sk = l2cap_get_chan_by_scid(&conn->chan_list, scid);
                 if (!sk)
-                        return 0;
+                        return -EFAULT;
         } else {
                 sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident);
                 if (!sk)
-                        return 0;
+                        return -EFAULT;
         }
 
         switch (result) {
@@ -2919,9 +3038,13 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
                 sk->sk_state = BT_CONFIG;
                 l2cap_pi(sk)->ident = 0;
                 l2cap_pi(sk)->dcid = dcid;
-                l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
                 l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND;
 
+                if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)
+                        break;
+
+                l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT;
+
                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
                                         l2cap_build_conf_req(sk, req), req);
                 l2cap_pi(sk)->num_conf_req++;
@@ -2957,8 +3080,14 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         if (!sk)
                 return -ENOENT;
 
-        if (sk->sk_state == BT_DISCONN)
+        if (sk->sk_state != BT_CONFIG) {
+                struct l2cap_cmd_rej rej;
+
+                rej.reason = cpu_to_le16(0x0002);
+                l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
+                                sizeof(rej), &rej);
                 goto unlock;
+        }
 
         /* Reject if config buffer is too small. */
         len = cmd_len - sizeof(*req);
@@ -2984,7 +3113,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         /* Complete config. */
         len = l2cap_parse_conf_req(sk, rsp);
         if (len < 0) {
-                l2cap_send_disconn_req(conn, sk);
+                l2cap_send_disconn_req(conn, sk, ECONNRESET);
                 goto unlock;
         }
 
@@ -3054,7 +3183,7 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                         char req[64];
 
                         if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) {
-                                l2cap_send_disconn_req(conn, sk);
+                                l2cap_send_disconn_req(conn, sk, ECONNRESET);
                                 goto done;
                         }
 
@@ -3063,7 +3192,7 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                         len = l2cap_parse_conf_rsp(sk, rsp->data,
                                                         len, req, &result);
                         if (len < 0) {
-                                l2cap_send_disconn_req(conn, sk);
+                                l2cap_send_disconn_req(conn, sk, ECONNRESET);
                                 goto done;
                         }
 
@@ -3076,10 +3205,9 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                 }
 
         default:
-                sk->sk_state = BT_DISCONN;
                 sk->sk_err = ECONNRESET;
                 l2cap_sock_set_timer(sk, HZ * 5);
-                l2cap_send_disconn_req(conn, sk);
+                l2cap_send_disconn_req(conn, sk, ECONNRESET);
                 goto done;
         }
 
@@ -3130,16 +3258,6 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
 
         sk->sk_shutdown = SHUTDOWN_MASK;
 
-        skb_queue_purge(TX_QUEUE(sk));
-
-        if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
-                skb_queue_purge(SREJ_QUEUE(sk));
-                skb_queue_purge(BUSY_QUEUE(sk));
-                del_timer(&l2cap_pi(sk)->retrans_timer);
-                del_timer(&l2cap_pi(sk)->monitor_timer);
-                del_timer(&l2cap_pi(sk)->ack_timer);
-        }
-
         l2cap_chan_del(sk, ECONNRESET);
         bh_unlock_sock(sk);
 
@@ -3162,16 +3280,6 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
         if (!sk)
                 return 0;
 
-        skb_queue_purge(TX_QUEUE(sk));
-
-        if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
-                skb_queue_purge(SREJ_QUEUE(sk));
-                skb_queue_purge(BUSY_QUEUE(sk));
-                del_timer(&l2cap_pi(sk)->retrans_timer);
-                del_timer(&l2cap_pi(sk)->monitor_timer);
-                del_timer(&l2cap_pi(sk)->ack_timer);
-        }
-
         l2cap_chan_del(sk, 0);
         bh_unlock_sock(sk);
 
@@ -3194,7 +3302,7 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
-                if (enable_ertm)
+                if (!disable_ertm)
                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
                                                          | L2CAP_FEAT_FCS;
                 put_unaligned_le32(feat_mask, rsp->data);
@@ -3359,7 +3467,7 @@ static int l2cap_check_fcs(struct l2cap_pinfo *pi,  struct sk_buff *skb)
                 our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
 
                 if (our_fcs != rcv_fcs)
-                        return -EINVAL;
+                        return -EBADMSG;
         }
         return 0;
 }
@@ -3370,25 +3478,19 @@ static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk)
         u16 control = 0;
 
         pi->frames_sent = 0;
-        pi->conn_state |= L2CAP_CONN_SEND_FBIT;
 
         control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
 
         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
-                control |= L2CAP_SUPER_RCV_NOT_READY | L2CAP_CTRL_FINAL;
+                control |= L2CAP_SUPER_RCV_NOT_READY;
                 l2cap_send_sframe(pi, control);
                 pi->conn_state |= L2CAP_CONN_RNR_SENT;
-                pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
         }
 
-        if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY && pi->unacked_frames > 0)
-                __mod_retrans_timer();
-
-        pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+        if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY)
+                l2cap_retransmit_frames(sk);
 
-        spin_lock_bh(&pi->send_lock);
         l2cap_ertm_send(sk);
-        spin_unlock_bh(&pi->send_lock);
 
         if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
                         pi->frames_sent == 0) {
@@ -3400,6 +3502,8 @@ static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk)
 static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
 {
         struct sk_buff *next_skb;
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        int tx_seq_offset, next_tx_seq_offset;
 
         bt_cb(skb)->tx_seq = tx_seq;
         bt_cb(skb)->sar = sar;
@@ -3410,11 +3514,20 @@ static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_s
                 return 0;
         }
 
+        tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
+        if (tx_seq_offset < 0)
+                tx_seq_offset += 64;
+
         do {
                 if (bt_cb(next_skb)->tx_seq == tx_seq)
                         return -EINVAL;
 
-                if (bt_cb(next_skb)->tx_seq > tx_seq) {
+                next_tx_seq_offset = (bt_cb(next_skb)->tx_seq -
+                                                pi->buffer_seq) % 64;
+                if (next_tx_seq_offset < 0)
+                        next_tx_seq_offset += 64;
+
+                if (next_tx_seq_offset > tx_seq_offset) {
                         __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
                         return 0;
                 }
@@ -3532,11 +3645,51 @@ drop:
         pi->sdu = NULL;
 
 disconnect:
-        l2cap_send_disconn_req(pi->conn, sk);
+        l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
         kfree_skb(skb);
         return 0;
 }
 
+static int l2cap_try_push_rx_skb(struct sock *sk)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        struct sk_buff *skb;
+        u16 control;
+        int err;
+
+        while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
+                control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
+                err = l2cap_ertm_reassembly_sdu(sk, skb, control);
+                if (err < 0) {
+                        skb_queue_head(BUSY_QUEUE(sk), skb);
+                        return -EBUSY;
+                }
+
+                pi->buffer_seq = (pi->buffer_seq + 1) % 64;
+        }
+
+        if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
+                goto done;
+
+        control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+        control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
+        l2cap_send_sframe(pi, control);
+        l2cap_pi(sk)->retry_count = 1;
+
+        del_timer(&pi->retrans_timer);
+        __mod_monitor_timer();
+
+        l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
+
+done:
+        pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
+        pi->conn_state &= ~L2CAP_CONN_RNR_SENT;
+
+        BT_DBG("sk %p, Exit local busy", sk);
+
+        return 0;
+}
+
 static void l2cap_busy_work(struct work_struct *work)
 {
         DECLARE_WAITQUEUE(wait, current);
@@ -3545,7 +3698,6 @@ static void l2cap_busy_work(struct work_struct *work)
         struct sock *sk = (struct sock *)pi;
         int n_tries = 0, timeo = HZ/5, err;
         struct sk_buff *skb;
-        u16 control;
 
         lock_sock(sk);
 
@@ -3555,8 +3707,8 @@ static void l2cap_busy_work(struct work_struct *work)
 
                 if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
                         err = -EBUSY;
-                        l2cap_send_disconn_req(pi->conn, sk);
-                        goto done;
+                        l2cap_send_disconn_req(pi->conn, sk, EBUSY);
+                        break;
                 }
 
                 if (!timeo)
@@ -3564,7 +3716,7 @@ static void l2cap_busy_work(struct work_struct *work)
 
                 if (signal_pending(current)) {
                         err = sock_intr_errno(timeo);
-                        goto done;
+                        break;
                 }
 
                 release_sock(sk);
@@ -3573,40 +3725,12 @@ static void l2cap_busy_work(struct work_struct *work)
 
                 err = sock_error(sk);
                 if (err)
-                        goto done;
-
-                while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
-                        control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
-                        err = l2cap_ertm_reassembly_sdu(sk, skb, control);
-                        if (err < 0) {
-                                skb_queue_head(BUSY_QUEUE(sk), skb);
-                                break;
-                        }
-
-                        pi->buffer_seq = (pi->buffer_seq + 1) % 64;
-                }
+                        break;
 
-                if (!skb)
+                if (l2cap_try_push_rx_skb(sk) == 0)
                         break;
         }
 
-        if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
-                goto done;
-
-        control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
-        control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
-        l2cap_send_sframe(pi, control);
-        l2cap_pi(sk)->retry_count = 1;
-
-        del_timer(&pi->retrans_timer);
-        __mod_monitor_timer();
-
-        l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
-
-done:
-        pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
-        pi->conn_state &= ~L2CAP_CONN_RNR_SENT;
-
         set_current_state(TASK_RUNNING);
         remove_wait_queue(sk_sleep(sk), &wait);
 
@@ -3621,7 +3745,9 @@ static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
         if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
                 bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
                 __skb_queue_tail(BUSY_QUEUE(sk), skb);
-                return -EBUSY;
+                return l2cap_try_push_rx_skb(sk);
+
+
         }
 
         err = l2cap_ertm_reassembly_sdu(sk, skb, control);
@@ -3631,6 +3757,8 @@ static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
         }
 
         /* Busy Condition */
+        BT_DBG("sk %p, Enter local busy", sk);
+
         pi->conn_state |= L2CAP_CONN_LOCAL_BUSY;
         bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
         __skb_queue_tail(BUSY_QUEUE(sk), skb);
@@ -3641,6 +3769,8 @@ static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
 
         pi->conn_state |= L2CAP_CONN_RNR_SENT;
 
+        del_timer(&pi->ack_timer);
+
         queue_work(_busy_wq, &pi->busy_work);
 
         return err;
@@ -3754,7 +3884,7 @@ static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
                 l2cap_ertm_reassembly_sdu(sk, skb, control);
                 l2cap_pi(sk)->buffer_seq_srej =
                         (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
-                tx_seq++;
+                tx_seq = (tx_seq + 1) % 64;
         }
 }
 
@@ -3790,10 +3920,11 @@ static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
                 l2cap_send_sframe(pi, control);
 
                 new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
-                new->tx_seq = pi->expected_tx_seq++;
+                new->tx_seq = pi->expected_tx_seq;
+                pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
                 list_add_tail(&new->list, SREJ_LIST(sk));
         }
-        pi->expected_tx_seq++;
+        pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
 }
 
 static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
@@ -3802,11 +3933,12 @@ static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, str
         u8 tx_seq = __get_txseq(rx_control);
         u8 req_seq = __get_reqseq(rx_control);
         u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
-        u8 tx_seq_offset, expected_tx_seq_offset;
+        int tx_seq_offset, expected_tx_seq_offset;
         int num_to_ack = (pi->tx_win/6) + 1;
         int err = 0;
 
-        BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
+        BT_DBG("sk %p len %d tx_seq %d rx_control 0x%4.4x", sk, skb->len, tx_seq,
+                                                                rx_control);
 
         if (L2CAP_CTRL_FINAL & rx_control &&
                         l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
@@ -3828,7 +3960,7 @@ static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, str
 
         /* invalid tx_seq */
         if (tx_seq_offset >= pi->tx_win) {
-                l2cap_send_disconn_req(pi->conn, sk);
+                l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
                 goto drop;
         }
 
@@ -3851,6 +3983,7 @@ static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, str
                                 pi->buffer_seq = pi->buffer_seq_srej;
                                 pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
                                 l2cap_send_ack(pi);
+                                BT_DBG("sk %p, Exit SREJ_SENT", sk);
                         }
                 } else {
                         struct srej_list *l;
@@ -3879,6 +4012,8 @@ static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, str
 
                 pi->conn_state |= L2CAP_CONN_SREJ_SENT;
 
+                BT_DBG("sk %p, Enter SREJ", sk);
+
                 INIT_LIST_HEAD(SREJ_LIST(sk));
                 pi->buffer_seq_srej = pi->buffer_seq;
 
@@ -3889,6 +4024,8 @@ static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, str
                 pi->conn_state |= L2CAP_CONN_SEND_PBIT;
 
                 l2cap_send_srejframe(sk, tx_seq);
+
+                del_timer(&pi->ack_timer);
         }
         return 0;
 
@@ -3902,6 +4039,10 @@ expected:
                 return 0;
         }
 
+        err = l2cap_push_rx_skb(sk, skb, rx_control);
+        if (err < 0)
+                return 0;
+
         if (rx_control & L2CAP_CTRL_FINAL) {
                 if (pi->conn_state & L2CAP_CONN_REJ_ACT)
                         pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
@@ -3909,10 +4050,6 @@ expected:
                         l2cap_retransmit_frames(sk);
         }
 
-        err = l2cap_push_rx_skb(sk, skb, rx_control);
-        if (err < 0)
-                return 0;
-
         __mod_ack_timer();
 
         pi->num_acked = (pi->num_acked + 1) % num_to_ack;
@@ -3930,10 +4067,14 @@ static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
 {
         struct l2cap_pinfo *pi = l2cap_pi(sk);
 
+        BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, __get_reqseq(rx_control),
+                                                rx_control);
+
         pi->expected_ack_seq = __get_reqseq(rx_control);
         l2cap_drop_acked_frames(sk);
 
         if (rx_control & L2CAP_CTRL_POLL) {
+                pi->conn_state |= L2CAP_CONN_SEND_FBIT;
                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
                         if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
                                         (pi->unacked_frames > 0))
@@ -3962,9 +4103,7 @@ static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
                 if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
                         l2cap_send_ack(pi);
                 } else {
-                        spin_lock_bh(&pi->send_lock);
                         l2cap_ertm_send(sk);
-                        spin_unlock_bh(&pi->send_lock);
                 }
         }
 }
@@ -3974,6 +4113,8 @@ static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control)
         struct l2cap_pinfo *pi = l2cap_pi(sk);
         u8 tx_seq = __get_reqseq(rx_control);
 
+        BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
+
         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
 
         pi->expected_ack_seq = tx_seq;
@@ -3996,16 +4137,18 @@ static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
         struct l2cap_pinfo *pi = l2cap_pi(sk);
         u8 tx_seq = __get_reqseq(rx_control);
 
+        BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
+
         pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
 
         if (rx_control & L2CAP_CTRL_POLL) {
                 pi->expected_ack_seq = tx_seq;
                 l2cap_drop_acked_frames(sk);
+
+                pi->conn_state |= L2CAP_CONN_SEND_FBIT;
                 l2cap_retransmit_one_frame(sk, tx_seq);
 
-                spin_lock_bh(&pi->send_lock);
                 l2cap_ertm_send(sk);
-                spin_unlock_bh(&pi->send_lock);
 
                 if (pi->conn_state & L2CAP_CONN_WAIT_F) {
                         pi->srej_save_reqseq = tx_seq;
@@ -4031,10 +4174,15 @@ static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control)
         struct l2cap_pinfo *pi = l2cap_pi(sk);
         u8 tx_seq = __get_reqseq(rx_control);
 
+        BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control);
+
         pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
         pi->expected_ack_seq = tx_seq;
         l2cap_drop_acked_frames(sk);
 
+        if (rx_control & L2CAP_CTRL_POLL)
+                pi->conn_state |= L2CAP_CONN_SEND_FBIT;
+
         if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
                 del_timer(&pi->retrans_timer);
                 if (rx_control & L2CAP_CTRL_POLL)
@@ -4082,12 +4230,83 @@ static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, str
         return 0;
 }
 
+static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        u16 control;
+        u8 req_seq;
+        int len, next_tx_seq_offset, req_seq_offset;
+
+        control = get_unaligned_le16(skb->data);
+        skb_pull(skb, 2);
+        len = skb->len;
+
+        /*
+         * We can just drop the corrupted I-frame here.
+         * Receiver will miss it and start proper recovery
+         * procedures and ask retransmission.
+         */
+        if (l2cap_check_fcs(pi, skb))
+                goto drop;
+
+        if (__is_sar_start(control) && __is_iframe(control))
+                len -= 2;
+
+        if (pi->fcs == L2CAP_FCS_CRC16)
+                len -= 2;
+
+        if (len > pi->mps) {
+                l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
+                goto drop;
+        }
+
+        req_seq = __get_reqseq(control);
+        req_seq_offset = (req_seq - pi->expected_ack_seq) % 64;
+        if (req_seq_offset < 0)
+                req_seq_offset += 64;
+
+        next_tx_seq_offset =
+                (pi->next_tx_seq - pi->expected_ack_seq) % 64;
+        if (next_tx_seq_offset < 0)
+                next_tx_seq_offset += 64;
+
+        /* check for invalid req-seq */
+        if (req_seq_offset > next_tx_seq_offset) {
+                l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
+                goto drop;
+        }
+
+        if (__is_iframe(control)) {
+                if (len < 0) {
+                        l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
+                        goto drop;
+                }
+
+                l2cap_data_channel_iframe(sk, control, skb);
+        } else {
+                if (len != 0) {
+                        BT_ERR("%d", len);
+                        l2cap_send_disconn_req(pi->conn, sk, ECONNRESET);
+                        goto drop;
+                }
+
+                l2cap_data_channel_sframe(sk, control, skb);
+        }
+
+        return 0;
+
+drop:
+        kfree_skb(skb);
+        return 0;
+}
+
 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
 {
         struct sock *sk;
         struct l2cap_pinfo *pi;
-        u16 control, len;
-        u8 tx_seq, req_seq, next_tx_seq_offset, req_seq_offset;
+        u16 control;
+        u8 tx_seq;
+        int len;
 
         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
         if (!sk) {
@@ -4117,59 +4336,11 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
                 break;
 
         case L2CAP_MODE_ERTM:
-                control = get_unaligned_le16(skb->data);
-                skb_pull(skb, 2);
-                len = skb->len;
-
-                if (__is_sar_start(control))
-                        len -= 2;
-
-                if (pi->fcs == L2CAP_FCS_CRC16)
-                        len -= 2;
-
-                /*
-                 * We can just drop the corrupted I-frame here.
-                 * Receiver will miss it and start proper recovery
-                 * procedures and ask retransmission.
-                 */
-                if (len > pi->mps) {
-                        l2cap_send_disconn_req(pi->conn, sk);
-                        goto drop;
-                }
-
-                if (l2cap_check_fcs(pi, skb))
-                        goto drop;
-
-                req_seq = __get_reqseq(control);
-                req_seq_offset = (req_seq - pi->expected_ack_seq) % 64;
-                if (req_seq_offset < 0)
-                        req_seq_offset += 64;
-
-                next_tx_seq_offset =
-                        (pi->next_tx_seq - pi->expected_ack_seq) % 64;
-                if (next_tx_seq_offset < 0)
-                        next_tx_seq_offset += 64;
-
-                /* check for invalid req-seq */
-                if (req_seq_offset > next_tx_seq_offset) {
-                        l2cap_send_disconn_req(pi->conn, sk);
-                        goto drop;
-                }
-
-                if (__is_iframe(control)) {
-                        if (len < 4) {
-                                l2cap_send_disconn_req(pi->conn, sk);
-                                goto drop;
-                        }
-
-                        l2cap_data_channel_iframe(sk, control, skb);
+                if (!sock_owned_by_user(sk)) {
+                        l2cap_ertm_data_rcv(sk, skb);
                 } else {
-                        if (len != 0) {
-                                l2cap_send_disconn_req(pi->conn, sk);
+                        if (sk_add_backlog(sk, skb))
                                 goto drop;
-                        }
-
-                        l2cap_data_channel_sframe(sk, control, skb);
                 }
 
                 goto done;
@@ -4179,16 +4350,16 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
                 skb_pull(skb, 2);
                 len = skb->len;
 
+                if (l2cap_check_fcs(pi, skb))
+                        goto drop;
+
                 if (__is_sar_start(control))
                         len -= 2;
 
                 if (pi->fcs == L2CAP_FCS_CRC16)
                         len -= 2;
 
-                if (len > pi->mps || len < 4 || __is_sframe(control))
-                        goto drop;
-
-                if (l2cap_check_fcs(pi, skb))
+                if (len > pi->mps || len < 0 || __is_sframe(control))
                         goto drop;
 
                 tx_seq = __get_txseq(control);
@@ -4288,7 +4459,7 @@ static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
         struct hlist_node *node;
 
         if (type != ACL_LINK)
-                return 0;
+                return -EINVAL;
 
         BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
 
@@ -4321,7 +4492,7 @@ static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
 
         if (hcon->type != ACL_LINK)
-                return 0;
+                return -EINVAL;
 
         if (!status) {
                 conn = l2cap_conn_add(hcon, status);
@@ -4350,7 +4521,7 @@ static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
         BT_DBG("hcon %p reason %d", hcon, reason);
 
         if (hcon->type != ACL_LINK)
-                return 0;
+                return -EINVAL;
 
         l2cap_conn_del(hcon, bt_err(reason));
 
@@ -4679,14 +4850,8 @@ EXPORT_SYMBOL(l2cap_load);
 module_init(l2cap_init);
 module_exit(l2cap_exit);
 
-module_param(enable_ertm, bool, 0644);
-MODULE_PARM_DESC(enable_ertm, "Enable enhanced retransmission mode");
-
-module_param(max_transmit, uint, 0644);
-MODULE_PARM_DESC(max_transmit, "Max transmit value (default = 3)");
-
-module_param(tx_window, uint, 0644);
-MODULE_PARM_DESC(tx_window, "Transmission window size value (default = 63)");
+module_param(disable_ertm, bool, 0644);
+MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode");
 
 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
diff --git a/net/bluetooth/rfcomm/sock.c b/net/bluetooth/rfcomm/sock.c
index 43fbf6b4b4bf..44a623275951 100644
--- a/net/bluetooth/rfcomm/sock.c
+++ b/net/bluetooth/rfcomm/sock.c
@@ -1152,7 +1152,7 @@ error:
         return err;
 }
 
-void rfcomm_cleanup_sockets(void)
+void __exit rfcomm_cleanup_sockets(void)
 {
         debugfs_remove(rfcomm_sock_debugfs);
 
diff --git a/net/bluetooth/rfcomm/tty.c b/net/bluetooth/rfcomm/tty.c
index 309b6c261b25..026205c18b78 100644
--- a/net/bluetooth/rfcomm/tty.c
+++ b/net/bluetooth/rfcomm/tty.c
@@ -1153,7 +1153,7 @@ static const struct tty_operations rfcomm_ops = {
         .tiocmset               = rfcomm_tty_tiocmset,
 };
 
-int rfcomm_init_ttys(void)
+int __init rfcomm_init_ttys(void)
 {
         rfcomm_tty_driver = alloc_tty_driver(RFCOMM_TTY_PORTS);
         if (!rfcomm_tty_driver)
@@ -1183,7 +1183,7 @@ int rfcomm_init_ttys(void)
         return 0;
 }
 
-void rfcomm_cleanup_ttys(void)
+void __exit rfcomm_cleanup_ttys(void)
 {
         tty_unregister_driver(rfcomm_tty_driver);
         put_tty_driver(rfcomm_tty_driver);
diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index db4d9340c846..29ac8e1a509e 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -158,7 +158,7 @@ static int ieee80211_add_key(struct wiphy *wiphy, struct net_device *dev,
         if (mac_addr) {
                 sta = sta_info_get_bss(sdata, mac_addr);
                 if (!sta) {
-                        ieee80211_key_free(key);
+                        ieee80211_key_free(sdata->local, key);
                         err = -ENOENT;
                         goto out_unlock;
                 }
@@ -192,7 +192,7 @@ static int ieee80211_del_key(struct wiphy *wiphy, struct net_device *dev,
                         goto out_unlock;
 
                 if (sta->key) {
-                        ieee80211_key_free(sta->key);
+                        ieee80211_key_free(sdata->local, sta->key);
                         WARN_ON(sta->key);
                         ret = 0;
                 }
@@ -205,7 +205,7 @@ static int ieee80211_del_key(struct wiphy *wiphy, struct net_device *dev,
                 goto out_unlock;
         }
 
-        ieee80211_key_free(sdata->keys[key_idx]);
+        ieee80211_key_free(sdata->local, sdata->keys[key_idx]);
         WARN_ON(sdata->keys[key_idx]);
 
         ret = 0;
@@ -324,15 +324,10 @@ static int ieee80211_config_default_mgmt_key(struct wiphy *wiphy,
                                              struct net_device *dev,
                                              u8 key_idx)
 {
-        struct ieee80211_sub_if_data *sdata;
-
-        rcu_read_lock();
+        struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
 
-        sdata = IEEE80211_DEV_TO_SUB_IF(dev);
         ieee80211_set_default_mgmt_key(sdata, key_idx);
 
-        rcu_read_unlock();
-
         return 0;
 }
 
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index ef470064b154..65e0ed6c2975 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -238,6 +238,7 @@ enum ieee80211_work_type {
         IEEE80211_WORK_ABORT,
         IEEE80211_WORK_DIRECT_PROBE,
         IEEE80211_WORK_AUTH,
+        IEEE80211_WORK_ASSOC_BEACON_WAIT,
         IEEE80211_WORK_ASSOC,
         IEEE80211_WORK_REMAIN_ON_CHANNEL,
 };
diff --git a/net/mac80211/key.c b/net/mac80211/key.c
index 50d1cff23d8e..1b9d87ed143a 100644
--- a/net/mac80211/key.c
+++ b/net/mac80211/key.c
@@ -323,13 +323,15 @@ static void __ieee80211_key_destroy(struct ieee80211_key *key)
         if (!key)
                 return;
 
-        ieee80211_key_disable_hw_accel(key);
+        if (key->local)
+                ieee80211_key_disable_hw_accel(key);
 
         if (key->conf.alg == ALG_CCMP)
                 ieee80211_aes_key_free(key->u.ccmp.tfm);
         if (key->conf.alg == ALG_AES_CMAC)
                 ieee80211_aes_cmac_key_free(key->u.aes_cmac.tfm);
-        ieee80211_debugfs_key_remove(key);
+        if (key->local)
+                ieee80211_debugfs_key_remove(key);
 
         kfree(key);
 }
@@ -410,15 +412,12 @@ static void __ieee80211_key_free(struct ieee80211_key *key)
         __ieee80211_key_destroy(key);
 }
 
-void ieee80211_key_free(struct ieee80211_key *key)
+void ieee80211_key_free(struct ieee80211_local *local,
+                        struct ieee80211_key *key)
 {
-        struct ieee80211_local *local;
-
         if (!key)
                 return;
 
-        local = key->sdata->local;
-
         mutex_lock(&local->key_mtx);
         __ieee80211_key_free(key);
         mutex_unlock(&local->key_mtx);
diff --git a/net/mac80211/key.h b/net/mac80211/key.h
index a3849fa3fce8..b665bbb7a471 100644
--- a/net/mac80211/key.h
+++ b/net/mac80211/key.h
@@ -135,7 +135,8 @@ struct ieee80211_key *ieee80211_key_alloc(enum ieee80211_key_alg alg,
 void ieee80211_key_link(struct ieee80211_key *key,
                         struct ieee80211_sub_if_data *sdata,
                         struct sta_info *sta);
-void ieee80211_key_free(struct ieee80211_key *key);
+void ieee80211_key_free(struct ieee80211_local *local,
+                        struct ieee80211_key *key);
 void ieee80211_set_default_key(struct ieee80211_sub_if_data *sdata, int idx);
 void ieee80211_set_default_mgmt_key(struct ieee80211_sub_if_data *sdata,
                                     int idx);
diff --git a/net/mac80211/main.c b/net/mac80211/main.c
index 0e95c750ded9..7cc4f913a431 100644
--- a/net/mac80211/main.c
+++ b/net/mac80211/main.c
@@ -107,12 +107,15 @@ int ieee80211_hw_config(struct ieee80211_local *local, u32 changed)
         if (scan_chan) {
                 chan = scan_chan;
                 channel_type = NL80211_CHAN_NO_HT;
+                local->hw.conf.flags |= IEEE80211_CONF_OFFCHANNEL;
         } else if (local->tmp_channel) {
                 chan = scan_chan = local->tmp_channel;
                 channel_type = local->tmp_channel_type;
+                local->hw.conf.flags |= IEEE80211_CONF_OFFCHANNEL;
         } else {
                 chan = local->oper_channel;
                 channel_type = local->_oper_channel_type;
+                local->hw.conf.flags &= ~IEEE80211_CONF_OFFCHANNEL;
         }
 
         if (chan != local->hw.conf.channel ||
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index cf8d72196c65..b6c163ac22da 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -870,6 +870,11 @@ static void ieee80211_set_associated(struct ieee80211_sub_if_data *sdata,
 
         ieee80211_led_assoc(local, 1);
 
+        if (local->hw.flags & IEEE80211_HW_NEED_DTIM_PERIOD)
+                bss_conf->dtim_period = bss->dtim_period;
+        else
+                bss_conf->dtim_period = 0;
+
         bss_conf->assoc = 1;
         /*
          * For now just always ask the driver to update the basic rateset
@@ -1751,7 +1756,8 @@ void ieee80211_sta_rx_queued_mgmt(struct ieee80211_sub_if_data *sdata,
                         if (wk->sdata != sdata)
                                 continue;
 
-                        if (wk->type != IEEE80211_WORK_ASSOC)
+                        if (wk->type != IEEE80211_WORK_ASSOC &&
+                            wk->type != IEEE80211_WORK_ASSOC_BEACON_WAIT)
                                 continue;
 
                         if (memcmp(mgmt->bssid, wk->filter_ta, ETH_ALEN))
@@ -2086,6 +2092,8 @@ static enum work_done_result ieee80211_assoc_done(struct ieee80211_work *wk,
                                                   struct sk_buff *skb)
 {
         struct ieee80211_mgmt *mgmt;
+        struct ieee80211_rx_status *rx_status;
+        struct ieee802_11_elems elems;
         u16 status;
 
         if (!skb) {
@@ -2093,6 +2101,19 @@ static enum work_done_result ieee80211_assoc_done(struct ieee80211_work *wk,
                 return WORK_DONE_DESTROY;
         }
 
+        if (wk->type == IEEE80211_WORK_ASSOC_BEACON_WAIT) {
+                mutex_lock(&wk->sdata->u.mgd.mtx);
+                rx_status = (void *) skb->cb;
+                ieee802_11_parse_elems(skb->data + 24 + 12, skb->len - 24 - 12, &elems);
+                ieee80211_rx_bss_info(wk->sdata, (void *)skb->data, skb->len, rx_status,
+                                      &elems, true);
+                mutex_unlock(&wk->sdata->u.mgd.mtx);
+
+                wk->type = IEEE80211_WORK_ASSOC;
+                /* not really done yet */
+                return WORK_DONE_REQUEUE;
+        }
+
         mgmt = (void *)skb->data;
         status = le16_to_cpu(mgmt->u.assoc_resp.status_code);
 
@@ -2206,10 +2227,14 @@ int ieee80211_mgd_assoc(struct ieee80211_sub_if_data *sdata,
         if (req->prev_bssid)
                 memcpy(wk->assoc.prev_bssid, req->prev_bssid, ETH_ALEN);
 
-        wk->type = IEEE80211_WORK_ASSOC;
         wk->chan = req->bss->channel;
         wk->sdata = sdata;
         wk->done = ieee80211_assoc_done;
+        if (!bss->dtim_period &&
+            sdata->local->hw.flags & IEEE80211_HW_NEED_DTIM_PERIOD)
+                wk->type = IEEE80211_WORK_ASSOC_BEACON_WAIT;
+        else
+                wk->type = IEEE80211_WORK_ASSOC;
 
         if (req->use_mfp) {
                 ifmgd->mfp = IEEE80211_MFP_REQUIRED;
@@ -2257,7 +2282,8 @@ int ieee80211_mgd_deauth(struct ieee80211_sub_if_data *sdata,
 
                         if (wk->type != IEEE80211_WORK_DIRECT_PROBE &&
                             wk->type != IEEE80211_WORK_AUTH &&
-                            wk->type != IEEE80211_WORK_ASSOC)
+                            wk->type != IEEE80211_WORK_ASSOC &&
+                            wk->type != IEEE80211_WORK_ASSOC_BEACON_WAIT)
                                 continue;
 
                         if (memcmp(req->bss->bssid, wk->filter_ta, ETH_ALEN))
diff --git a/net/mac80211/rc80211_minstrel.c b/net/mac80211/rc80211_minstrel.c
index f65ce6dcc8e2..778c604d7939 100644
--- a/net/mac80211/rc80211_minstrel.c
+++ b/net/mac80211/rc80211_minstrel.c
@@ -67,7 +67,6 @@ rix_to_ndx(struct minstrel_sta_info *mi, int rix)
         for (i = rix; i >= 0; i--)
                 if (mi->r[i].rix == rix)
                         break;
-        WARN_ON(i < 0);
         return i;
 }
 
diff --git a/net/mac80211/rc80211_minstrel_ht.c b/net/mac80211/rc80211_minstrel_ht.c
index b5ace243546c..c5b465904e3b 100644
--- a/net/mac80211/rc80211_minstrel_ht.c
+++ b/net/mac80211/rc80211_minstrel_ht.c
@@ -636,7 +636,7 @@ minstrel_ht_update_caps(void *priv, struct ieee80211_supported_band *sband,
         int i;
 
         /* fall back to the old minstrel for legacy stations */
-        if (sta && !sta->ht_cap.ht_supported) {
+        if (!sta->ht_cap.ht_supported) {
                 msp->is_ht = false;
                 memset(&msp->legacy, 0, sizeof(msp->legacy));
                 msp->legacy.r = msp->ratelist;
@@ -748,7 +748,7 @@ minstrel_ht_alloc_sta(void *priv, struct ieee80211_sta *sta, gfp_t gfp)
         return msp;
 
 error1:
-        kfree(msp->sample_table);
+        kfree(msp->ratelist);
 error:
         kfree(msp);
         return NULL;
diff --git a/net/mac80211/scan.c b/net/mac80211/scan.c
index 439c98d93a79..41f20fb7e670 100644
--- a/net/mac80211/scan.c
+++ b/net/mac80211/scan.c
@@ -114,6 +114,10 @@ ieee80211_bss_info_update(struct ieee80211_local *local,
                 bss->dtim_period = tim_ie->dtim_period;
         }
 
+        /* If the beacon had no TIM IE, or it was invalid, use 1 */
+        if (beacon && !bss->dtim_period)
+                bss->dtim_period = 1;
+
         /* replace old supported rates if we get new values */
         srlen = 0;
         if (elems->supp_rates) {
@@ -286,8 +290,6 @@ void ieee80211_scan_completed(struct ieee80211_hw *hw, bool aborted)
         local->scanning = 0;
         local->scan_channel = NULL;
 
-        drv_sw_scan_complete(local);
-
         /* we only have to protect scan_req and hw/sw scan */
         mutex_unlock(&local->scan_mtx);
 
@@ -297,6 +299,8 @@ void ieee80211_scan_completed(struct ieee80211_hw *hw, bool aborted)
 
         ieee80211_configure_filter(local);
 
+        drv_sw_scan_complete(local);
+
         ieee80211_offchannel_return(local, true);
 
  done:
diff --git a/net/mac80211/sta_info.c b/net/mac80211/sta_info.c
index 67656cbf2b15..6d86f0c1ad04 100644
--- a/net/mac80211/sta_info.c
+++ b/net/mac80211/sta_info.c
@@ -647,7 +647,7 @@ static int __must_check __sta_info_destroy(struct sta_info *sta)
                 return ret;
 
         if (sta->key) {
-                ieee80211_key_free(sta->key);
+                ieee80211_key_free(local, sta->key);
                 WARN_ON(sta->key);
         }
 
diff --git a/net/mac80211/tx.c b/net/mac80211/tx.c
index 698d4718b1a4..c54db966926b 100644
--- a/net/mac80211/tx.c
+++ b/net/mac80211/tx.c
@@ -576,17 +576,6 @@ ieee80211_tx_h_select_key(struct ieee80211_tx_data *tx)
 }
 
 static ieee80211_tx_result debug_noinline
-ieee80211_tx_h_sta(struct ieee80211_tx_data *tx)
-{
-        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
-
-        if (tx->sta && tx->sta->uploaded)
-                info->control.sta = &tx->sta->sta;
-
-        return TX_CONTINUE;
-}
-
-static ieee80211_tx_result debug_noinline
 ieee80211_tx_h_rate_ctrl(struct ieee80211_tx_data *tx)
 {
         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx->skb);
@@ -1307,6 +1296,11 @@ static int __ieee80211_tx(struct ieee80211_local *local,
                         break;
                 }
 
+                if (sta && sta->uploaded)
+                        info->control.sta = &sta->sta;
+                else
+                        info->control.sta = NULL;
+
                 ret = drv_tx(local, skb);
                 if (WARN_ON(ret != NETDEV_TX_OK && skb->len != len)) {
                         dev_kfree_skb(skb);
@@ -1346,7 +1340,6 @@ static int invoke_tx_handlers(struct ieee80211_tx_data *tx)
         CALL_TXH(ieee80211_tx_h_check_assoc);
         CALL_TXH(ieee80211_tx_h_ps_buf);
         CALL_TXH(ieee80211_tx_h_select_key);
-        CALL_TXH(ieee80211_tx_h_sta);
         if (!(tx->local->hw.flags & IEEE80211_HW_HAS_RATE_CONTROL))
                 CALL_TXH(ieee80211_tx_h_rate_ctrl);
 
@@ -1942,11 +1935,13 @@ netdev_tx_t ieee80211_subif_start_xmit(struct sk_buff *skb,
                 h_pos += encaps_len;
         }
 
+#ifdef CONFIG_MAC80211_MESH
         if (meshhdrlen > 0) {
                 memcpy(skb_push(skb, meshhdrlen), &mesh_hdr, meshhdrlen);
                 nh_pos += meshhdrlen;
                 h_pos += meshhdrlen;
         }
+#endif
 
         if (ieee80211_is_data_qos(fc)) {
                 __le16 *qos_control;
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index 794792177376..748387d45bc0 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -803,8 +803,12 @@ void ieee80211_set_wmm_default(struct ieee80211_sub_if_data *sdata)
 
         /* after reinitialize QoS TX queues setting to default,
          * disable QoS at all */
-        sdata->vif.bss_conf.qos = sdata->vif.type != NL80211_IFTYPE_STATION;
-        ieee80211_bss_info_change_notify(sdata, BSS_CHANGED_QOS);
+
+        if (sdata->vif.type != NL80211_IFTYPE_MONITOR) {
+                sdata->vif.bss_conf.qos =
+                        sdata->vif.type != NL80211_IFTYPE_STATION;
+                ieee80211_bss_info_change_notify(sdata, BSS_CHANGED_QOS);
+        }
 }
 
 void ieee80211_sta_def_wmm_params(struct ieee80211_sub_if_data *sdata,
diff --git a/net/mac80211/work.c b/net/mac80211/work.c
index c22a71c5cb45..81d4ad64184a 100644
--- a/net/mac80211/work.c
+++ b/net/mac80211/work.c
@@ -560,6 +560,22 @@ ieee80211_remain_on_channel_timeout(struct ieee80211_work *wk)
         return WORK_ACT_TIMEOUT;
 }
 
+static enum work_action __must_check
+ieee80211_assoc_beacon_wait(struct ieee80211_work *wk)
+{
+        if (wk->started)
+                return WORK_ACT_TIMEOUT;
+
+        /*
+         * Wait up to one beacon interval ...
+         * should this be more if we miss one?
+         */
+        printk(KERN_DEBUG "%s: waiting for beacon from %pM\n",
+               wk->sdata->name, wk->filter_ta);
+        wk->timeout = TU_TO_EXP_TIME(wk->assoc.bss->beacon_interval);
+        return WORK_ACT_NONE;
+}
+
 static void ieee80211_auth_challenge(struct ieee80211_work *wk,
                                      struct ieee80211_mgmt *mgmt,
                                      size_t len)
@@ -709,6 +725,25 @@ ieee80211_rx_mgmt_probe_resp(struct ieee80211_work *wk,
         return WORK_ACT_DONE;
 }
 
+static enum work_action __must_check
+ieee80211_rx_mgmt_beacon(struct ieee80211_work *wk,
+                         struct ieee80211_mgmt *mgmt, size_t len)
+{
+        struct ieee80211_sub_if_data *sdata = wk->sdata;
+        struct ieee80211_local *local = sdata->local;
+
+        ASSERT_WORK_MTX(local);
+
+        if (wk->type != IEEE80211_WORK_ASSOC_BEACON_WAIT)
+                return WORK_ACT_MISMATCH;
+
+        if (len < 24 + 12)
+                return WORK_ACT_NONE;
+
+        printk(KERN_DEBUG "%s: beacon received\n", sdata->name);
+        return WORK_ACT_DONE;
+}
+
 static void ieee80211_work_rx_queued_mgmt(struct ieee80211_local *local,
                                           struct sk_buff *skb)
 {
@@ -731,6 +766,7 @@ static void ieee80211_work_rx_queued_mgmt(struct ieee80211_local *local,
                 case IEEE80211_WORK_DIRECT_PROBE:
                 case IEEE80211_WORK_AUTH:
                 case IEEE80211_WORK_ASSOC:
+                case IEEE80211_WORK_ASSOC_BEACON_WAIT:
                         bssid = wk->filter_ta;
                         break;
                 default:
@@ -745,6 +781,9 @@ static void ieee80211_work_rx_queued_mgmt(struct ieee80211_local *local,
                         continue;
 
                 switch (fc & IEEE80211_FCTL_STYPE) {
+                case IEEE80211_STYPE_BEACON:
+                        rma = ieee80211_rx_mgmt_beacon(wk, mgmt, skb->len);
+                        break;
                 case IEEE80211_STYPE_PROBE_RESP:
                         rma = ieee80211_rx_mgmt_probe_resp(wk, mgmt, skb->len,
                                                            rx_status);
@@ -916,6 +955,9 @@ static void ieee80211_work_work(struct work_struct *work)
                 case IEEE80211_WORK_REMAIN_ON_CHANNEL:
                         rma = ieee80211_remain_on_channel_timeout(wk);
                         break;
+                case IEEE80211_WORK_ASSOC_BEACON_WAIT:
+                        rma = ieee80211_assoc_beacon_wait(wk);
+                        break;
                 }
 
                 wk->started = started;
@@ -1065,6 +1107,7 @@ ieee80211_rx_result ieee80211_work_rx_mgmt(struct ieee80211_sub_if_data *sdata,
                 case IEEE80211_STYPE_PROBE_RESP:
                 case IEEE80211_STYPE_ASSOC_RESP:
                 case IEEE80211_STYPE_REASSOC_RESP:
+                case IEEE80211_STYPE_BEACON:
                         skb_queue_tail(&local->work_skb_queue, skb);
                         ieee80211_queue_work(&local->hw, &local->work_work);
                         return RX_QUEUED;
diff --git a/net/wireless/core.c b/net/wireless/core.c
index f65c6494ede9..541e2fff5e9c 100644
--- a/net/wireless/core.c
+++ b/net/wireless/core.c
@@ -907,3 +907,52 @@ static void __exit cfg80211_exit(void)
         destroy_workqueue(cfg80211_wq);
 }
 module_exit(cfg80211_exit);
+
+static int ___wiphy_printk(const char *level, const struct wiphy *wiphy,
+                           struct va_format *vaf)
+{
+        if (!wiphy)
+                return printk("%s(NULL wiphy *): %pV", level, vaf);
+
+        return printk("%s%s: %pV", level, wiphy_name(wiphy), vaf);
+}
+
+int __wiphy_printk(const char *level, const struct wiphy *wiphy,
+                   const char *fmt, ...)
+{
+        struct va_format vaf;
+        va_list args;
+        int r;
+
+        va_start(args, fmt);
+
+        vaf.fmt = fmt;
+        vaf.va = &args;
+
+        r = ___wiphy_printk(level, wiphy, &vaf);
+        va_end(args);
+
+        return r;
+}
+EXPORT_SYMBOL(__wiphy_printk);
+
+#define define_wiphy_printk_level(func, kern_level)             \
+int func(const struct wiphy *wiphy, const char *fmt, ...)       \
+{                                                               \
+        struct va_format vaf;                                   \
+        va_list args;                                           \
+        int r;                                                  \
+                                                                \
+        va_start(args, fmt);                                    \
+                                                                \
+        vaf.fmt = fmt;                                          \
+        vaf.va = &args;                                         \
+                                                                \
+        r = ___wiphy_printk(kern_level, wiphy, &vaf);           \
+        va_end(args);                                           \
+                                                                \
+        return r;                                               \
+}                                                               \
+EXPORT_SYMBOL(func);
+
+define_wiphy_printk_level(wiphy_debug, KERN_DEBUG);
diff --git a/net/wireless/ibss.c b/net/wireless/ibss.c
index adcabba02e20..27a8ce9343c3 100644
--- a/net/wireless/ibss.c
+++ b/net/wireless/ibss.c
@@ -247,8 +247,10 @@ int cfg80211_ibss_wext_join(struct cfg80211_registered_device *rdev,
         if (!netif_running(wdev->netdev))
                 return 0;
 
-        if (wdev->wext.keys)
+        if (wdev->wext.keys) {
                 wdev->wext.keys->def = wdev->wext.default_key;
+                wdev->wext.keys->defmgmt = wdev->wext.default_mgmt_key;
+        }
 
         wdev->wext.ibss.privacy = wdev->wext.default_key != -1;
 
diff --git a/net/wireless/lib80211_crypt_ccmp.c b/net/wireless/lib80211_crypt_ccmp.c
index b7fa31d5fd13..dacb3b4b1bdb 100644
--- a/net/wireless/lib80211_crypt_ccmp.c
+++ b/net/wireless/lib80211_crypt_ccmp.c
@@ -467,7 +467,6 @@ static struct lib80211_crypto_ops lib80211_crypt_ccmp = {
         .name = "CCMP",
         .init = lib80211_ccmp_init,
         .deinit = lib80211_ccmp_deinit,
-        .build_iv = lib80211_ccmp_hdr,
         .encrypt_mpdu = lib80211_ccmp_encrypt,
         .decrypt_mpdu = lib80211_ccmp_decrypt,
         .encrypt_msdu = NULL,
diff --git a/net/wireless/lib80211_crypt_tkip.c b/net/wireless/lib80211_crypt_tkip.c
index a7f995613f1f..0fe40510e2cb 100644
--- a/net/wireless/lib80211_crypt_tkip.c
+++ b/net/wireless/lib80211_crypt_tkip.c
@@ -757,7 +757,6 @@ static struct lib80211_crypto_ops lib80211_crypt_tkip = {
         .name = "TKIP",
         .init = lib80211_tkip_init,
         .deinit = lib80211_tkip_deinit,
-        .build_iv = lib80211_tkip_hdr,
         .encrypt_mpdu = lib80211_tkip_encrypt,
         .decrypt_mpdu = lib80211_tkip_decrypt,
         .encrypt_msdu = lib80211_michael_mic_add,
diff --git a/net/wireless/lib80211_crypt_wep.c b/net/wireless/lib80211_crypt_wep.c
index 6d41e05ca33b..e2e88878ba35 100644
--- a/net/wireless/lib80211_crypt_wep.c
+++ b/net/wireless/lib80211_crypt_wep.c
@@ -269,7 +269,6 @@ static struct lib80211_crypto_ops lib80211_crypt_wep = {
         .name = "WEP",
         .init = lib80211_wep_init,
         .deinit = lib80211_wep_deinit,
-        .build_iv = lib80211_wep_build_iv,
         .encrypt_mpdu = lib80211_wep_encrypt,
         .decrypt_mpdu = lib80211_wep_decrypt,
         .encrypt_msdu = NULL,
diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c
index fbfac588297c..37902a54e9c1 100644
--- a/net/wireless/nl80211.c
+++ b/net/wireless/nl80211.c
@@ -2769,6 +2769,7 @@ static int nl80211_get_mesh_params(struct sk_buff *skb,
 
  nla_put_failure:
         genlmsg_cancel(msg, hdr);
+        nlmsg_free(msg);
         err = -EMSGSIZE;
  out:
         /* Cleanup */
@@ -2960,6 +2961,7 @@ static int nl80211_get_reg(struct sk_buff *skb, struct genl_info *info)
 
 nla_put_failure:
         genlmsg_cancel(msg, hdr);
+        nlmsg_free(msg);
         err = -EMSGSIZE;
 out:
         mutex_unlock(&cfg80211_mutex);
diff --git a/net/wireless/reg.c b/net/wireless/reg.c
index 48baf28cc4b6..f180db0de66c 100644
--- a/net/wireless/reg.c
+++ b/net/wireless/reg.c
@@ -1306,6 +1306,7 @@ static void reg_process_hint(struct regulatory_request *reg_request)
 {
         int r = 0;
         struct wiphy *wiphy = NULL;
+        enum nl80211_reg_initiator initiator = reg_request->initiator;
 
         BUG_ON(!reg_request->alpha2);
 
@@ -1325,7 +1326,7 @@ static void reg_process_hint(struct regulatory_request *reg_request)
         /* This is required so that the orig_* parameters are saved */
         if (r == -EALREADY && wiphy &&
             wiphy->flags & WIPHY_FLAG_STRICT_REGULATORY)
-                wiphy_update_regulatory(wiphy, reg_request->initiator);
+                wiphy_update_regulatory(wiphy, initiator);
 out:
         mutex_unlock(&reg_mutex);
         mutex_unlock(&cfg80211_mutex);
@@ -1492,7 +1493,6 @@ void regulatory_hint_11d(struct wiphy *wiphy,
                          u8 *country_ie,
                          u8 country_ie_len)
 {
-        struct ieee80211_regdomain *rd = NULL;
         char alpha2[2];
         enum environment_cap env = ENVIRON_ANY;
         struct regulatory_request *request;
@@ -1529,7 +1529,7 @@ void regulatory_hint_11d(struct wiphy *wiphy,
 
         request = kzalloc(sizeof(struct regulatory_request), GFP_KERNEL);
         if (!request)
-                goto free_rd_out;
+                goto out;
 
         request->wiphy_idx = get_wiphy_idx(wiphy);
         request->alpha2[0] = alpha2[0];
@@ -1543,8 +1543,6 @@ void regulatory_hint_11d(struct wiphy *wiphy,
 
         return;
 
-free_rd_out:
-        kfree(rd);
 out:
         mutex_unlock(&reg_mutex);
 }