Merge remote-tracking branch 'wireless-next/master' into mac80211-next

48 files changed, 2237 insertions, 552 deletions

diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 3537d385035..1c11d0dcd86 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -11,6 +11,7 @@ menuconfig BT
         select CRYPTO_BLKCIPHER
         select CRYPTO_AES
         select CRYPTO_ECB
+        select CRYPTO_SHA256
         help
           Bluetooth is low-cost, low-power, short-range wireless technology.
           It was designed as a replacement for cables and other short-range
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a4602..dea6a287dac 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
         hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-        a2mp.o
+        a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0760d1fed6f..d5136cfb57e 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,11 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+/* Global AMP Manager list */
+LIST_HEAD(amp_mgr_list);
+DEFINE_MUTEX(amp_mgr_list_lock);
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -37,8 +42,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
         return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-                      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
         struct l2cap_chan *chan = mgr->a2mp_chan;
         struct a2mp_cmd *cmd;
@@ -63,6 +67,14 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
         kfree(cmd);
 }
 
+u8 __next_ident(struct amp_mgr *mgr)
+{
+        if (++mgr->ident == 0)
+                mgr->ident = 1;
+
+        return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
         cl->id = 0;
@@ -161,6 +173,83 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
         return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+                             struct a2mp_cmd *hdr)
+{
+        struct a2mp_discov_rsp *rsp = (void *) skb->data;
+        u16 len = le16_to_cpu(hdr->len);
+        struct a2mp_cl *cl;
+        u16 ext_feat;
+        bool found = false;
+
+        if (len < sizeof(*rsp))
+                return -EINVAL;
+
+        len -= sizeof(*rsp);
+        skb_pull(skb, sizeof(*rsp));
+
+        ext_feat = le16_to_cpu(rsp->ext_feat);
+
+        BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
+
+        /* check that packet is not broken for now */
+        while (ext_feat & A2MP_FEAT_EXT) {
+                if (len < sizeof(ext_feat))
+                        return -EINVAL;
+
+                ext_feat = get_unaligned_le16(skb->data);
+                BT_DBG("efm 0x%4.4x", ext_feat);
+                len -= sizeof(ext_feat);
+                skb_pull(skb, sizeof(ext_feat));
+        }
+
+        cl = (void *) skb->data;
+        while (len >= sizeof(*cl)) {
+                BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+                       cl->status);
+
+                if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+                        struct a2mp_info_req req;
+
+                        found = true;
+                        req.id = cl->id;
+                        a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+                                  sizeof(req), &req);
+                }
+
+                len -= sizeof(*cl);
+                cl = (void *) skb_pull(skb, sizeof(*cl));
+        }
+
+        /* Fall back to L2CAP init sequence */
+        if (!found) {
+                struct l2cap_conn *conn = mgr->l2cap_conn;
+                struct l2cap_chan *chan;
+
+                mutex_lock(&conn->chan_lock);
+
+                list_for_each_entry(chan, &conn->chan_l, list) {
+
+                        BT_DBG("chan %p state %s", chan,
+                               state_to_string(chan->state));
+
+                        if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+                                continue;
+
+                        l2cap_chan_lock(chan);
+
+                        if (chan->state == BT_CONNECT)
+                                l2cap_send_conn_req(chan);
+
+                        l2cap_chan_unlock(chan);
+                }
+
+                mutex_unlock(&conn->chan_lock);
+        }
+
+        return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
                               struct a2mp_cmd *hdr)
 {
@@ -181,7 +270,6 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
                             struct a2mp_cmd *hdr)
 {
         struct a2mp_info_req *req  = (void *) skb->data;
-        struct a2mp_info_rsp rsp;
         struct hci_dev *hdev;
 
         if (le16_to_cpu(hdr->len) < sizeof(*req))
@@ -189,53 +277,93 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
         BT_DBG("id %d", req->id);
 
-        rsp.id = req->id;
-        rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
-
         hdev = hci_dev_get(req->id);
-        if (hdev && hdev->amp_type != HCI_BREDR) {
-                rsp.status = 0;
-                rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
-                rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
-                rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
-                rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
-                rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+        if (!hdev || hdev->dev_type != HCI_AMP) {
+                struct a2mp_info_rsp rsp;
+
+                rsp.id = req->id;
+                rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+                a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
+                          &rsp);
+
+                goto done;
         }
 
+        mgr->state = READ_LOC_AMP_INFO;
+        hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
+
+done:
         if (hdev)
                 hci_dev_put(hdev);
 
-        a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
-
         skb_pull(skb, sizeof(*req));
         return 0;
 }
 
+static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+                            struct a2mp_cmd *hdr)
+{
+        struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+        struct a2mp_amp_assoc_req req;
+        struct amp_ctrl *ctrl;
+
+        if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+                return -EINVAL;
+
+        BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
+
+        if (rsp->status)
+                return -EINVAL;
+
+        ctrl = amp_ctrl_add(mgr, rsp->id);
+        if (!ctrl)
+                return -ENOMEM;
+
+        req.id = rsp->id;
+        a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+                  &req);
+
+        skb_pull(skb, sizeof(*rsp));
+        return 0;
+}
+
 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
                                 struct a2mp_cmd *hdr)
 {
         struct a2mp_amp_assoc_req *req = (void *) skb->data;
         struct hci_dev *hdev;
+        struct amp_mgr *tmp;
 
         if (le16_to_cpu(hdr->len) < sizeof(*req))
                 return -EINVAL;
 
         BT_DBG("id %d", req->id);
 
+        /* Make sure that other request is not processed */
+        tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+
         hdev = hci_dev_get(req->id);
-        if (!hdev || hdev->amp_type == HCI_BREDR) {
+        if (!hdev || hdev->amp_type == HCI_BREDR || tmp) {
                 struct a2mp_amp_assoc_rsp rsp;
                 rsp.id = req->id;
-                rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+                if (tmp) {
+                        rsp.status = A2MP_STATUS_COLLISION_OCCURED;
+                        amp_mgr_put(tmp);
+                } else {
+                        rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+                }
 
                 a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
                           &rsp);
-                goto clean;
+
+                goto done;
         }
 
-        /* Placeholder for HCI Read AMP Assoc */
+        amp_read_loc_assoc(hdev, mgr);
 
-clean:
+done:
         if (hdev)
                 hci_dev_put(hdev);
 
@@ -243,6 +371,68 @@ clean:
         return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+                                struct a2mp_cmd *hdr)
+{
+        struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
+        u16 len = le16_to_cpu(hdr->len);
+        struct hci_dev *hdev;
+        struct amp_ctrl *ctrl;
+        struct hci_conn *hcon;
+        size_t assoc_len;
+
+        if (len < sizeof(*rsp))
+                return -EINVAL;
+
+        assoc_len = len - sizeof(*rsp);
+
+        BT_DBG("id %d status 0x%2.2x assoc len %zu", rsp->id, rsp->status,
+               assoc_len);
+
+        if (rsp->status)
+                return -EINVAL;
+
+        /* Save remote ASSOC data */
+        ctrl = amp_ctrl_lookup(mgr, rsp->id);
+        if (ctrl) {
+                u8 *assoc;
+
+                assoc = kzalloc(assoc_len, GFP_KERNEL);
+                if (!assoc) {
+                        amp_ctrl_put(ctrl);
+                        return -ENOMEM;
+                }
+
+                memcpy(assoc, rsp->amp_assoc, assoc_len);
+                ctrl->assoc = assoc;
+                ctrl->assoc_len = assoc_len;
+                ctrl->assoc_rem_len = assoc_len;
+                ctrl->assoc_len_so_far = 0;
+
+                amp_ctrl_put(ctrl);
+        }
+
+        /* Create Phys Link */
+        hdev = hci_dev_get(rsp->id);
+        if (!hdev)
+                return -EINVAL;
+
+        hcon = phylink_add(hdev, mgr, rsp->id, true);
+        if (!hcon)
+                goto done;
+
+        BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
+
+        mgr->bredr_chan->ctrl_id = rsp->id;
+
+        amp_create_phylink(hdev, mgr, hcon);
+
+done:
+        hci_dev_put(hdev);
+        skb_pull(skb, len);
+        return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
                                    struct a2mp_cmd *hdr)
 {
@@ -250,6 +440,8 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
         struct a2mp_physlink_rsp rsp;
         struct hci_dev *hdev;
+        struct hci_conn *hcon;
+        struct amp_ctrl *ctrl;
 
         if (le16_to_cpu(hdr->len) < sizeof(*req))
                 return -EINVAL;
@@ -265,9 +457,43 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
                 goto send_rsp;
         }
 
-        /* TODO process physlink create */
+        ctrl = amp_ctrl_lookup(mgr, rsp.remote_id);
+        if (!ctrl) {
+                ctrl = amp_ctrl_add(mgr, rsp.remote_id);
+                if (ctrl) {
+                        amp_ctrl_get(ctrl);
+                } else {
+                        rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
+                        goto send_rsp;
+                }
+        }
 
-        rsp.status = A2MP_STATUS_SUCCESS;
+        if (ctrl) {
+                size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req);
+                u8 *assoc;
+
+                assoc = kzalloc(assoc_len, GFP_KERNEL);
+                if (!assoc) {
+                        amp_ctrl_put(ctrl);
+                        return -ENOMEM;
+                }
+
+                memcpy(assoc, req->amp_assoc, assoc_len);
+                ctrl->assoc = assoc;
+                ctrl->assoc_len = assoc_len;
+                ctrl->assoc_rem_len = assoc_len;
+                ctrl->assoc_len_so_far = 0;
+
+                amp_ctrl_put(ctrl);
+        }
+
+        hcon = phylink_add(hdev, mgr, req->local_id, false);
+        if (hcon) {
+                amp_accept_phylink(hdev, mgr, hcon);
+                rsp.status = A2MP_STATUS_SUCCESS;
+        } else {
+                rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
+        }
 
 send_rsp:
         if (hdev)
@@ -286,6 +512,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
         struct a2mp_physlink_req *req = (void *) skb->data;
         struct a2mp_physlink_rsp rsp;
         struct hci_dev *hdev;
+        struct hci_conn *hcon;
 
         if (le16_to_cpu(hdr->len) < sizeof(*req))
                 return -EINVAL;
@@ -296,14 +523,22 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
         rsp.remote_id = req->local_id;
         rsp.status = A2MP_STATUS_SUCCESS;
 
-        hdev = hci_dev_get(req->local_id);
+        hdev = hci_dev_get(req->remote_id);
         if (!hdev) {
                 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
                 goto send_rsp;
         }
 
+        hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst);
+        if (!hcon) {
+                BT_ERR("No phys link exist");
+                rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+                goto clean;
+        }
+
         /* TODO Disconnect Phys Link here */
 
+clean:
         hci_dev_put(hdev);
 
 send_rsp:
@@ -377,10 +612,19 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
                         err = a2mp_discphyslink_req(mgr, skb, hdr);
                         break;
 
-                case A2MP_CHANGE_RSP:
                 case A2MP_DISCOVER_RSP:
+                        err = a2mp_discover_rsp(mgr, skb, hdr);
+                        break;
+
                 case A2MP_GETINFO_RSP:
+                        err = a2mp_getinfo_rsp(mgr, skb, hdr);
+                        break;
+
                 case A2MP_GETAMPASSOC_RSP:
+                        err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+                        break;
+
+                case A2MP_CHANGE_RSP:
                 case A2MP_CREATEPHYSLINK_RSP:
                 case A2MP_DISCONNPHYSLINK_RSP:
                         err = a2mp_cmd_rsp(mgr, skb, hdr);
@@ -455,9 +699,10 @@ static struct l2cap_ops a2mp_chan_ops = {
         .new_connection = l2cap_chan_no_new_connection,
         .teardown = l2cap_chan_no_teardown,
         .ready = l2cap_chan_no_ready,
+        .defer = l2cap_chan_no_defer,
 };
 
-static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
+static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
 {
         struct l2cap_chan *chan;
         int err;
@@ -492,7 +737,10 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
         chan->conf_state = 0;
 
-        l2cap_chan_add(conn, chan);
+        if (locked)
+                __l2cap_chan_add(conn, chan);
+        else
+                l2cap_chan_add(conn, chan);
 
         chan->remote_mps = chan->omtu;
         chan->mps = chan->omtu;
@@ -503,11 +751,13 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 }
 
 /* AMP Manager functions */
-void amp_mgr_get(struct amp_mgr *mgr)
+struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr)
 {
         BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
 
         kref_get(&mgr->kref);
+
+        return mgr;
 }
 
 static void amp_mgr_destroy(struct kref *kref)
@@ -516,6 +766,11 @@ static void amp_mgr_destroy(struct kref *kref)
 
         BT_DBG("mgr %p", mgr);
 
+        mutex_lock(&amp_mgr_list_lock);
+        list_del(&mgr->list);
+        mutex_unlock(&amp_mgr_list_lock);
+
+        amp_ctrl_list_flush(mgr);
         kfree(mgr);
 }
 
@@ -526,7 +781,7 @@ int amp_mgr_put(struct amp_mgr *mgr)
         return kref_put(&mgr->kref, &amp_mgr_destroy);
 }
 
-static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
 {
         struct amp_mgr *mgr;
         struct l2cap_chan *chan;
@@ -539,7 +794,7 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
         mgr->l2cap_conn = conn;
 
-        chan = a2mp_chan_open(conn);
+        chan = a2mp_chan_open(conn, locked);
         if (!chan) {
                 kfree(mgr);
                 return NULL;
@@ -552,6 +807,14 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
         kref_init(&mgr->kref);
 
+        /* Remote AMP ctrl list initialization */
+        INIT_LIST_HEAD(&mgr->amp_ctrls);
+        mutex_init(&mgr->amp_ctrls_lock);
+
+        mutex_lock(&amp_mgr_list_lock);
+        list_add(&mgr->list, &amp_mgr_list);
+        mutex_unlock(&amp_mgr_list_lock);
+
         return mgr;
 }
 
@@ -560,7 +823,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 {
         struct amp_mgr *mgr;
 
-        mgr = amp_mgr_create(conn);
+        mgr = amp_mgr_create(conn, false);
         if (!mgr) {
                 BT_ERR("Could not create AMP manager");
                 return NULL;
@@ -570,3 +833,139 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
         return mgr->a2mp_chan;
 }
+
+struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
+{
+        struct amp_mgr *mgr;
+
+        mutex_lock(&amp_mgr_list_lock);
+        list_for_each_entry(mgr, &amp_mgr_list, list) {
+                if (mgr->state == state) {
+                        amp_mgr_get(mgr);
+                        mutex_unlock(&amp_mgr_list_lock);
+                        return mgr;
+                }
+        }
+        mutex_unlock(&amp_mgr_list_lock);
+
+        return NULL;
+}
+
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
+{
+        struct amp_mgr *mgr;
+        struct a2mp_info_rsp rsp;
+
+        mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
+        if (!mgr)
+                return;
+
+        BT_DBG("%s mgr %p", hdev->name, mgr);
+
+        rsp.id = hdev->id;
+        rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+        if (hdev->amp_type != HCI_BREDR) {
+                rsp.status = 0;
+                rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+                rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+                rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+                rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+                rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+        }
+
+        a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+        amp_mgr_put(mgr);
+}
+
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
+{
+        struct amp_mgr *mgr;
+        struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+        struct a2mp_amp_assoc_rsp *rsp;
+        size_t len;
+
+        mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+        if (!mgr)
+                return;
+
+        BT_DBG("%s mgr %p", hdev->name, mgr);
+
+        len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+        rsp = kzalloc(len, GFP_KERNEL);
+        if (!rsp) {
+                amp_mgr_put(mgr);
+                return;
+        }
+
+        rsp->id = hdev->id;
+
+        if (status) {
+                rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+        } else {
+                rsp->status = A2MP_STATUS_SUCCESS;
+                memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+        }
+
+        a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+        amp_mgr_put(mgr);
+        kfree(rsp);
+}
+
+void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
+{
+        struct amp_mgr *mgr;
+        struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+        struct a2mp_physlink_req *req;
+        struct l2cap_chan *bredr_chan;
+        size_t len;
+
+        mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
+        if (!mgr)
+                return;
+
+        len = sizeof(*req) + loc_assoc->len;
+
+        BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);
+
+        req = kzalloc(len, GFP_KERNEL);
+        if (!req) {
+                amp_mgr_put(mgr);
+                return;
+        }
+
+        bredr_chan = mgr->bredr_chan;
+        if (!bredr_chan)
+                goto clean;
+
+        req->local_id = hdev->id;
+        req->remote_id = bredr_chan->ctrl_id;
+        memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+        a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
+
+clean:
+        amp_mgr_put(mgr);
+        kfree(req);
+}
+
+void a2mp_discover_amp(struct l2cap_chan *chan)
+{
+        struct l2cap_conn *conn = chan->conn;
+        struct amp_mgr *mgr = conn->hcon->amp_mgr;
+        struct a2mp_discov_req req;
+
+        BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
+
+        if (!mgr) {
+                mgr = amp_mgr_create(conn, true);
+                if (!mgr)
+                        return;
+        }
+
+        mgr->bredr_chan = chan;
+
+        req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+        req.ext_feat = 0;
+        a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+}
diff --git a/net/bluetooth/af_bluetooth.c b/net/bluetooth/af_bluetooth.c
index ba033f09196..5355df63d39 100644
--- a/net/bluetooth/af_bluetooth.c
+++ b/net/bluetooth/af_bluetooth.c
@@ -569,7 +569,6 @@ static int bt_seq_show(struct seq_file *seq, void *v)
 {
         struct bt_seq_state *s = seq->private;
         struct bt_sock_list *l = s->l;
-        bdaddr_t src_baswapped, dst_baswapped;
 
         if (v == SEQ_START_TOKEN) {
                 seq_puts(seq ,"sk               RefCnt Rmem   Wmem   User   Inode  Src Dst Parent");
@@ -583,18 +582,17 @@ static int bt_seq_show(struct seq_file *seq, void *v)
         } else {
                 struct sock *sk = sk_entry(v);
                 struct bt_sock *bt = bt_sk(sk);
-                baswap(&src_baswapped, &bt->src);
-                baswap(&dst_baswapped, &bt->dst);
 
-                seq_printf(seq, "%pK %-6d %-6u %-6u %-6u %-6lu %pM %pM %-6lu",
+                seq_printf(seq,
+                           "%pK %-6d %-6u %-6u %-6u %-6lu %pMR %pMR %-6lu",
                            sk,
                            atomic_read(&sk->sk_refcnt),
                            sk_rmem_alloc_get(sk),
                            sk_wmem_alloc_get(sk),
                            from_kuid(seq_user_ns(seq), sock_i_uid(sk)),
                            sock_i_ino(sk),
-                           &src_baswapped,
-                           &dst_baswapped,
+                           &bt->src,
+                           &bt->dst,
                            bt->parent? sock_i_ino(bt->parent): 0LU);
 
                 if (l->custom_seq_show) {
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 00000000000..231d7ef53ec
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,374 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+#include <crypto/hash.h>
+
+/* Remote AMP Controllers interface */
+void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+        BT_DBG("ctrl %p orig refcnt %d", ctrl,
+               atomic_read(&ctrl->kref.refcount));
+
+        kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+        struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+        BT_DBG("ctrl %p", ctrl);
+
+        kfree(ctrl->assoc);
+        kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+        BT_DBG("ctrl %p orig refcnt %d", ctrl,
+               atomic_read(&ctrl->kref.refcount));
+
+        return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr, u8 id)
+{
+        struct amp_ctrl *ctrl;
+
+        ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+        if (!ctrl)
+                return NULL;
+
+        kref_init(&ctrl->kref);
+        ctrl->id = id;
+
+        mutex_lock(&mgr->amp_ctrls_lock);
+        list_add(&ctrl->list, &mgr->amp_ctrls);
+        mutex_unlock(&mgr->amp_ctrls_lock);
+
+        BT_DBG("mgr %p ctrl %p", mgr, ctrl);
+
+        return ctrl;
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+        struct amp_ctrl *ctrl, *n;
+
+        BT_DBG("mgr %p", mgr);
+
+        mutex_lock(&mgr->amp_ctrls_lock);
+        list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+                list_del(&ctrl->list);
+                amp_ctrl_put(ctrl);
+        }
+        mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+        struct amp_ctrl *ctrl;
+
+        BT_DBG("mgr %p id %d", mgr, id);
+
+        mutex_lock(&mgr->amp_ctrls_lock);
+        list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+                if (ctrl->id == id) {
+                        amp_ctrl_get(ctrl);
+                        mutex_unlock(&mgr->amp_ctrls_lock);
+                        return ctrl;
+                }
+        }
+        mutex_unlock(&mgr->amp_ctrls_lock);
+
+        return NULL;
+}
+
+/* Physical Link interface */
+static u8 __next_handle(struct amp_mgr *mgr)
+{
+        if (++mgr->handle == 0)
+                mgr->handle = 1;
+
+        return mgr->handle;
+}
+
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+                             u8 remote_id, bool out)
+{
+        bdaddr_t *dst = mgr->l2cap_conn->dst;
+        struct hci_conn *hcon;
+
+        hcon = hci_conn_add(hdev, AMP_LINK, dst);
+        if (!hcon)
+                return NULL;
+
+        BT_DBG("hcon %p dst %pMR", hcon, dst);
+
+        hcon->state = BT_CONNECT;
+        hcon->attempt++;
+        hcon->handle = __next_handle(mgr);
+        hcon->remote_id = remote_id;
+        hcon->amp_mgr = amp_mgr_get(mgr);
+        hcon->out = out;
+
+        return hcon;
+}
+
+/* AMP crypto key generation interface */
+static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+        int ret = 0;
+        struct crypto_shash *tfm;
+
+        if (!ksize)
+                return -EINVAL;
+
+        tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+        if (IS_ERR(tfm)) {
+                BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm));
+                return PTR_ERR(tfm);
+        }
+
+        ret = crypto_shash_setkey(tfm, key, ksize);
+        if (ret) {
+                BT_DBG("crypto_ahash_setkey failed: err %d", ret);
+        } else {
+                struct {
+                        struct shash_desc shash;
+                        char ctx[crypto_shash_descsize(tfm)];
+                } desc;
+
+                desc.shash.tfm = tfm;
+                desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+                ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+                                          output);
+        }
+
+        crypto_free_shash(tfm);
+        return ret;
+}
+
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+        struct hci_dev *hdev = conn->hdev;
+        struct link_key *key;
+        u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+        u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+        int err;
+
+        if (!hci_conn_check_link_mode(conn))
+                return -EACCES;
+
+        BT_DBG("conn %p key_type %d", conn, conn->key_type);
+
+        /* Legacy key */
+        if (conn->key_type < 3) {
+                BT_ERR("Legacy key type %d", conn->key_type);
+                return -EACCES;
+        }
+
+        *type = conn->key_type;
+        *len = HCI_AMP_LINK_KEY_SIZE;
+
+        key = hci_find_link_key(hdev, &conn->dst);
+        if (!key) {
+                BT_DBG("No Link key for conn %p dst %pMR", conn, &conn->dst);
+                return -EACCES;
+        }
+
+        /* BR/EDR Link Key concatenated together with itself */
+        memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+        memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+        /* Derive Generic AMP Link Key (gamp) */
+        err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
+        if (err) {
+                BT_ERR("Could not derive Generic AMP Key: err %d", err);
+                return err;
+        }
+
+        if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
+                BT_DBG("Use Generic AMP Key (gamp)");
+                memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+                return err;
+        }
+
+        /* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
+        return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
+}
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+        struct hci_cp_read_local_amp_assoc cp;
+        struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+        BT_DBG("%s handle %d", hdev->name, phy_handle);
+
+        cp.phy_handle = phy_handle;
+        cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+        cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+        hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+        struct hci_cp_read_local_amp_assoc cp;
+
+        memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+        memset(&cp, 0, sizeof(cp));
+
+        cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+        mgr->state = READ_LOC_AMP_ASSOC;
+        hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+void amp_read_loc_assoc_final_data(struct hci_dev *hdev,
+                                   struct hci_conn *hcon)
+{
+        struct hci_cp_read_local_amp_assoc cp;
+        struct amp_mgr *mgr = hcon->amp_mgr;
+
+        cp.phy_handle = hcon->handle;
+        cp.len_so_far = cpu_to_le16(0);
+        cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+        mgr->state = READ_LOC_AMP_ASSOC_FINAL;
+
+        /* Read Local AMP Assoc final link information data */
+        hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+/* Write AMP Assoc data fragments, returns true with last fragment written*/
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev,
+                                     struct hci_conn *hcon)
+{
+        struct hci_cp_write_remote_amp_assoc *cp;
+        struct amp_mgr *mgr = hcon->amp_mgr;
+        struct amp_ctrl *ctrl;
+        u16 frag_len, len;
+
+        ctrl = amp_ctrl_lookup(mgr, hcon->remote_id);
+        if (!ctrl)
+                return false;
+
+        if (!ctrl->assoc_rem_len) {
+                BT_DBG("all fragments are written");
+                ctrl->assoc_rem_len = ctrl->assoc_len;
+                ctrl->assoc_len_so_far = 0;
+
+                amp_ctrl_put(ctrl);
+                return true;
+        }
+
+        frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
+        len = frag_len + sizeof(*cp);
+
+        cp = kzalloc(len, GFP_KERNEL);
+        if (!cp) {
+                amp_ctrl_put(ctrl);
+                return false;
+        }
+
+        BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u",
+               hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
+
+        cp->phy_handle = hcon->handle;
+        cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
+        cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
+        memcpy(cp->frag, ctrl->assoc, frag_len);
+
+        ctrl->assoc_len_so_far += frag_len;
+        ctrl->assoc_rem_len -= frag_len;
+
+        amp_ctrl_put(ctrl);
+
+        hci_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
+
+        kfree(cp);
+
+        return false;
+}
+
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
+{
+        struct hci_conn *hcon;
+
+        BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+        hcon = hci_conn_hash_lookup_handle(hdev, handle);
+        if (!hcon)
+                return;
+
+        amp_write_rem_assoc_frag(hdev, hcon);
+}
+
+void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle)
+{
+        struct hci_conn *hcon;
+
+        BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+        hcon = hci_conn_hash_lookup_handle(hdev, handle);
+        if (!hcon)
+                return;
+
+        BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon);
+
+        amp_write_rem_assoc_frag(hdev, hcon);
+}
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+                        struct hci_conn *hcon)
+{
+        struct hci_cp_create_phy_link cp;
+
+        cp.phy_handle = hcon->handle;
+
+        BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
+               hcon->handle);
+
+        if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+                            &cp.key_type)) {
+                BT_DBG("Cannot create link key");
+                return;
+        }
+
+        hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
+}
+
+void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+                        struct hci_conn *hcon)
+{
+        struct hci_cp_accept_phy_link cp;
+
+        cp.phy_handle = hcon->handle;
+
+        BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
+               hcon->handle);
+
+        if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+                            &cp.key_type)) {
+                BT_DBG("Cannot create link key");
+                return;
+        }
+
+        hci_send_cmd(hdev, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/bnep/core.c b/net/bluetooth/bnep/core.c
index 4a6620bc157..a5b63970263 100644
--- a/net/bluetooth/bnep/core.c
+++ b/net/bluetooth/bnep/core.c
@@ -182,8 +182,7 @@ static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
                         a2 = data;
                         data += ETH_ALEN;
 
-                        BT_DBG("mc filter %s -> %s",
-                                batostr((void *) a1), batostr((void *) a2));
+                        BT_DBG("mc filter %pMR -> %pMR", a1, a2);
 
                         /* Iterate from a1 to a2 */
                         set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
diff --git a/net/bluetooth/cmtp/core.c b/net/bluetooth/cmtp/core.c
index 6c9c1fd601c..e0a6ebf2baa 100644
--- a/net/bluetooth/cmtp/core.c
+++ b/net/bluetooth/cmtp/core.c
@@ -353,7 +353,7 @@ int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
 
         BT_DBG("mtu %d", session->mtu);
 
-        sprintf(session->name, "%s", batostr(&bt_sk(sock->sk)->dst));
+        sprintf(session->name, "%pMR", &bt_sk(sock->sk)->dst);
 
         session->sock  = sock;
         session->state = BT_CONFIG;
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index b9196a44f75..fe646211c61 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -130,6 +130,20 @@ void hci_acl_disconn(struct hci_conn *conn, __u8 reason)
         hci_send_cmd(conn->hdev, HCI_OP_DISCONNECT, sizeof(cp), &cp);
 }
 
+static void hci_amp_disconn(struct hci_conn *conn, __u8 reason)
+{
+        struct hci_cp_disconn_phy_link cp;
+
+        BT_DBG("hcon %p", conn);
+
+        conn->state = BT_DISCONN;
+
+        cp.phy_handle = HCI_PHY_HANDLE(conn->handle);
+        cp.reason = reason;
+        hci_send_cmd(conn->hdev, HCI_OP_DISCONN_PHY_LINK,
+                     sizeof(cp), &cp);
+}
+
 static void hci_add_sco(struct hci_conn *conn, __u16 handle)
 {
         struct hci_dev *hdev = conn->hdev;
@@ -230,11 +244,24 @@ void hci_sco_setup(struct hci_conn *conn, __u8 status)
         }
 }
 
+static void hci_conn_disconnect(struct hci_conn *conn)
+{
+        __u8 reason = hci_proto_disconn_ind(conn);
+
+        switch (conn->type) {
+        case ACL_LINK:
+                hci_acl_disconn(conn, reason);
+                break;
+        case AMP_LINK:
+                hci_amp_disconn(conn, reason);
+                break;
+        }
+}
+
 static void hci_conn_timeout(struct work_struct *work)
 {
         struct hci_conn *conn = container_of(work, struct hci_conn,
                                              disc_work.work);
-        __u8 reason;
 
         BT_DBG("hcon %p state %s", conn, state_to_string(conn->state));
 
@@ -253,8 +280,7 @@ static void hci_conn_timeout(struct work_struct *work)
                 break;
         case BT_CONFIG:
         case BT_CONNECTED:
-                reason = hci_proto_disconn_ind(conn);
-                hci_acl_disconn(conn, reason);
+                hci_conn_disconnect(conn);
                 break;
         default:
                 conn->state = BT_CLOSED;
@@ -320,7 +346,7 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
 {
         struct hci_conn *conn;
 
-        BT_DBG("%s dst %s", hdev->name, batostr(dst));
+        BT_DBG("%s dst %pMR", hdev->name, dst);
 
         conn = kzalloc(sizeof(struct hci_conn), GFP_KERNEL);
         if (!conn)
@@ -437,7 +463,7 @@ struct hci_dev *hci_get_route(bdaddr_t *dst, bdaddr_t *src)
         int use_src = bacmp(src, BDADDR_ANY);
         struct hci_dev *hdev = NULL, *d;
 
-        BT_DBG("%s -> %s", batostr(src), batostr(dst));
+        BT_DBG("%pMR -> %pMR", src, dst);
 
         read_lock(&hci_dev_list_lock);
 
@@ -567,7 +593,7 @@ static struct hci_conn *hci_connect_sco(struct hci_dev *hdev, int type,
 struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst,
                              __u8 dst_type, __u8 sec_level, __u8 auth_type)
 {
-        BT_DBG("%s dst %s type 0x%x", hdev->name, batostr(dst), type);
+        BT_DBG("%s dst %pMR type 0x%x", hdev->name, dst, type);
 
         switch (type) {
         case LE_LINK:
@@ -963,3 +989,35 @@ void hci_chan_list_flush(struct hci_conn *conn)
         list_for_each_entry_safe(chan, n, &conn->chan_list, list)
                 hci_chan_del(chan);
 }
+
+static struct hci_chan *__hci_chan_lookup_handle(struct hci_conn *hcon,
+                                                 __u16 handle)
+{
+        struct hci_chan *hchan;
+
+        list_for_each_entry(hchan, &hcon->chan_list, list) {
+                if (hchan->handle == handle)
+                        return hchan;
+        }
+
+        return NULL;
+}
+
+struct hci_chan *hci_chan_lookup_handle(struct hci_dev *hdev, __u16 handle)
+{
+        struct hci_conn_hash *h = &hdev->conn_hash;
+        struct hci_conn *hcon;
+        struct hci_chan *hchan = NULL;
+
+        rcu_read_lock();
+
+        list_for_each_entry_rcu(hcon, &h->list, list) {
+                hchan = __hci_chan_lookup_handle(hcon, handle);
+                if (hchan)
+                        break;
+        }
+
+        rcu_read_unlock();
+
+        return hchan;
+}
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 8a0ce706aeb..5a3f941b610 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -405,7 +405,7 @@ struct inquiry_entry *hci_inquiry_cache_lookup(struct hci_dev *hdev,
         struct discovery_state *cache = &hdev->discovery;
         struct inquiry_entry *e;
 
-        BT_DBG("cache %p, %s", cache, batostr(bdaddr));
+        BT_DBG("cache %p, %pMR", cache, bdaddr);
 
         list_for_each_entry(e, &cache->all, all) {
                 if (!bacmp(&e->data.bdaddr, bdaddr))
@@ -421,7 +421,7 @@ struct inquiry_entry *hci_inquiry_cache_lookup_unknown(struct hci_dev *hdev,
         struct discovery_state *cache = &hdev->discovery;
         struct inquiry_entry *e;
 
-        BT_DBG("cache %p, %s", cache, batostr(bdaddr));
+        BT_DBG("cache %p, %pMR", cache, bdaddr);
 
         list_for_each_entry(e, &cache->unknown, list) {
                 if (!bacmp(&e->data.bdaddr, bdaddr))
@@ -438,7 +438,7 @@ struct inquiry_entry *hci_inquiry_cache_lookup_resolve(struct hci_dev *hdev,
         struct discovery_state *cache = &hdev->discovery;
         struct inquiry_entry *e;
 
-        BT_DBG("cache %p bdaddr %s state %d", cache, batostr(bdaddr), state);
+        BT_DBG("cache %p bdaddr %pMR state %d", cache, bdaddr, state);
 
         list_for_each_entry(e, &cache->resolve, list) {
                 if (!bacmp(bdaddr, BDADDR_ANY) && e->name_state == state)
@@ -475,7 +475,7 @@ bool hci_inquiry_cache_update(struct hci_dev *hdev, struct inquiry_data *data,
         struct discovery_state *cache = &hdev->discovery;
         struct inquiry_entry *ie;
 
-        BT_DBG("cache %p, %s", cache, batostr(&data->bdaddr));
+        BT_DBG("cache %p, %pMR", cache, &data->bdaddr);
 
         if (ssp)
                 *ssp = data->ssp_mode;
@@ -1259,7 +1259,7 @@ int hci_add_link_key(struct hci_dev *hdev, struct hci_conn *conn, int new_key,
                 list_add(&key->list, &hdev->link_keys);
         }
 
-        BT_DBG("%s key for %s type %u", hdev->name, batostr(bdaddr), type);
+        BT_DBG("%s key for %pMR type %u", hdev->name, bdaddr, type);
 
         /* Some buggy controller combinations generate a changed
          * combination key for legacy pairing even when there's no
@@ -1338,7 +1338,7 @@ int hci_remove_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr)
         if (!key)
                 return -ENOENT;
 
-        BT_DBG("%s removing %s", hdev->name, batostr(bdaddr));
+        BT_DBG("%s removing %pMR", hdev->name, bdaddr);
 
         list_del(&key->list);
         kfree(key);
@@ -1354,7 +1354,7 @@ int hci_remove_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr)
                 if (bacmp(bdaddr, &k->bdaddr))
                         continue;
 
-                BT_DBG("%s removing %s", hdev->name, batostr(bdaddr));
+                BT_DBG("%s removing %pMR", hdev->name, bdaddr);
 
                 list_del(&k->list);
                 kfree(k);
@@ -1401,7 +1401,7 @@ int hci_remove_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr)
         if (!data)
                 return -ENOENT;
 
-        BT_DBG("%s removing %s", hdev->name, batostr(bdaddr));
+        BT_DBG("%s removing %pMR", hdev->name, bdaddr);
 
         list_del(&data->list);
         kfree(data);
@@ -1440,7 +1440,7 @@ int hci_add_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 *hash,
         memcpy(data->hash, hash, sizeof(data->hash));
         memcpy(data->randomizer, randomizer, sizeof(data->randomizer));
 
-        BT_DBG("%s for %s", hdev->name, batostr(bdaddr));
+        BT_DBG("%s for %pMR", hdev->name, bdaddr);
 
         return 0;
 }
@@ -2153,9 +2153,10 @@ static void hci_add_acl_hdr(struct sk_buff *skb, __u16 handle, __u16 flags)
         hdr->dlen   = cpu_to_le16(len);
 }
 
-static void hci_queue_acl(struct hci_conn *conn, struct sk_buff_head *queue,
+static void hci_queue_acl(struct hci_chan *chan, struct sk_buff_head *queue,
                           struct sk_buff *skb, __u16 flags)
 {
+        struct hci_conn *conn = chan->conn;
         struct hci_dev *hdev = conn->hdev;
         struct sk_buff *list;
 
@@ -2163,7 +2164,18 @@ static void hci_queue_acl(struct hci_conn *conn, struct sk_buff_head *queue,
         skb->data_len = 0;
 
         bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT;
-        hci_add_acl_hdr(skb, conn->handle, flags);
+
+        switch (hdev->dev_type) {
+        case HCI_BREDR:
+                hci_add_acl_hdr(skb, conn->handle, flags);
+                break;
+        case HCI_AMP:
+                hci_add_acl_hdr(skb, chan->handle, flags);
+                break;
+        default:
+                BT_ERR("%s unknown dev_type %d", hdev->name, hdev->dev_type);
+                return;
+        }
 
         list = skb_shinfo(skb)->frag_list;
         if (!list) {
@@ -2202,14 +2214,13 @@ static void hci_queue_acl(struct hci_conn *conn, struct sk_buff_head *queue,
 
 void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags)
 {
-        struct hci_conn *conn = chan->conn;
-        struct hci_dev *hdev = conn->hdev;
+        struct hci_dev *hdev = chan->conn->hdev;
 
         BT_DBG("%s chan %p flags 0x%4.4x", hdev->name, chan, flags);
 
         skb->dev = (void *) hdev;
 
-        hci_queue_acl(conn, &chan->data_q, skb, flags);
+        hci_queue_acl(chan, &chan->data_q, skb, flags);
 
         queue_work(hdev->workqueue, &hdev->tx_work);
 }
@@ -2311,8 +2322,8 @@ static void hci_link_tx_to(struct hci_dev *hdev, __u8 type)
         /* Kill stalled connections */
         list_for_each_entry_rcu(c, &h->list, list) {
                 if (c->type == type && c->sent) {
-                        BT_ERR("%s killing stalled connection %s",
-                               hdev->name, batostr(&c->dst));
+                        BT_ERR("%s killing stalled connection %pMR",
+                               hdev->name, &c->dst);
                         hci_acl_disconn(c, HCI_ERROR_REMOTE_USER_TERM);
                 }
         }
@@ -2381,6 +2392,9 @@ static struct hci_chan *hci_chan_sent(struct hci_dev *hdev, __u8 type,
         case ACL_LINK:
                 cnt = hdev->acl_cnt;
                 break;
+        case AMP_LINK:
+                cnt = hdev->block_cnt;
+                break;
         case SCO_LINK:
         case ESCO_LINK:
                 cnt = hdev->sco_cnt;
@@ -2510,11 +2524,19 @@ static void hci_sched_acl_blk(struct hci_dev *hdev)
         struct hci_chan *chan;
         struct sk_buff *skb;
         int quote;
+        u8 type;
 
         __check_timeout(hdev, cnt);
 
+        BT_DBG("%s", hdev->name);
+
+        if (hdev->dev_type == HCI_AMP)
+                type = AMP_LINK;
+        else
+                type = ACL_LINK;
+
         while (hdev->block_cnt > 0 &&
-               (chan = hci_chan_sent(hdev, ACL_LINK, &quote))) {
+               (chan = hci_chan_sent(hdev, type, &quote))) {
                 u32 priority = (skb_peek(&chan->data_q))->priority;
                 while (quote > 0 && (skb = skb_peek(&chan->data_q))) {
                         int blocks;
@@ -2547,14 +2569,19 @@ static void hci_sched_acl_blk(struct hci_dev *hdev)
         }
 
         if (cnt != hdev->block_cnt)
-                hci_prio_recalculate(hdev, ACL_LINK);
+                hci_prio_recalculate(hdev, type);
 }
 
 static void hci_sched_acl(struct hci_dev *hdev)
 {
         BT_DBG("%s", hdev->name);
 
-        if (!hci_conn_num(hdev, ACL_LINK))
+        /* No ACL link over BR/EDR controller */
+        if (!hci_conn_num(hdev, ACL_LINK) && hdev->dev_type == HCI_BREDR)
+                return;
+
+        /* No AMP link over AMP controller */
+        if (!hci_conn_num(hdev, AMP_LINK) && hdev->dev_type == HCI_AMP)
                 return;
 
         switch (hdev->flow_ctl_mode) {
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 2022b43c735..0383635f91f 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,8 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -846,7 +848,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
         BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 
         if (rp->status)
-                return;
+                goto a2mp_rsp;
 
         hdev->amp_status = rp->amp_status;
         hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -860,6 +862,46 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
         hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
         hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+a2mp_rsp:
+        a2mp_send_getinfo_rsp(hdev);
+}
+
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+                                        struct sk_buff *skb)
+{
+        struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
+        struct amp_assoc *assoc = &hdev->loc_assoc;
+        size_t rem_len, frag_len;
+
+        BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+        if (rp->status)
+                goto a2mp_rsp;
+
+        frag_len = skb->len - sizeof(*rp);
+        rem_len = __le16_to_cpu(rp->rem_len);
+
+        if (rem_len > frag_len) {
+                BT_DBG("frag_len %zu rem_len %zu", frag_len, rem_len);
+
+                memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+                assoc->offset += frag_len;
+
+                /* Read other fragments */
+                amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+                return;
+        }
+
+        memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+        assoc->len = assoc->offset + rem_len;
+        assoc->offset = 0;
+
+a2mp_rsp:
+        /* Send A2MP Rsp when all fragments are received */
+        a2mp_send_getampassoc_rsp(hdev, rp->status);
+        a2mp_send_create_phy_link_req(hdev, rp->status);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
@@ -1174,6 +1216,20 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
         hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status);
 }
 
+static void hci_cc_write_remote_amp_assoc(struct hci_dev *hdev,
+                                          struct sk_buff *skb)
+{
+        struct hci_rp_write_remote_amp_assoc *rp = (void *) skb->data;
+
+        BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
+               hdev->name, rp->status, rp->phy_handle);
+
+        if (rp->status)
+                return;
+
+        amp_write_rem_assoc_continue(hdev, rp->phy_handle);
+}
+
 static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
         BT_DBG("%s status 0x%2.2x", hdev->name, status);
@@ -1210,7 +1266,7 @@ static void hci_cs_create_conn(struct hci_dev *hdev, __u8 status)
 
         conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr);
 
-        BT_DBG("%s bdaddr %s hcon %p", hdev->name, batostr(&cp->bdaddr), conn);
+        BT_DBG("%s bdaddr %pMR hcon %p", hdev->name, &cp->bdaddr, conn);
 
         if (status) {
                 if (conn && conn->state == BT_CONNECT) {
@@ -1639,8 +1695,7 @@ static void hci_cs_le_create_conn(struct hci_dev *hdev, __u8 status)
                         return;
                 }
 
-                BT_DBG("%s bdaddr %s conn %p", hdev->name, batostr(&conn->dst),
-                       conn);
+                BT_DBG("%s bdaddr %pMR conn %p", hdev->name, &conn->dst, conn);
 
                 conn->state = BT_CLOSED;
                 mgmt_connect_failed(hdev, &conn->dst, conn->type,
@@ -1657,6 +1712,38 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
         BT_DBG("%s status 0x%2.2x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
+{
+        struct hci_cp_create_phy_link *cp;
+
+        BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+        if (status)
+                return;
+
+        cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_PHY_LINK);
+        if (!cp)
+                return;
+
+        amp_write_remote_assoc(hdev, cp->phy_handle);
+}
+
+static void hci_cs_accept_phylink(struct hci_dev *hdev, u8 status)
+{
+        struct hci_cp_accept_phy_link *cp;
+
+        BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+        if (status)
+                return;
+
+        cp = hci_sent_cmd_data(hdev, HCI_OP_ACCEPT_PHY_LINK);
+        if (!cp)
+                return;
+
+        amp_write_remote_assoc(hdev, cp->phy_handle);
+}
+
 static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         __u8 status = *((__u8 *) skb->data);
@@ -1822,7 +1909,7 @@ static void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
         struct hci_ev_conn_request *ev = (void *) skb->data;
         int mask = hdev->link_mode;
 
-        BT_DBG("%s bdaddr %s type 0x%x", hdev->name, batostr(&ev->bdaddr),
+        BT_DBG("%s bdaddr %pMR type 0x%x", hdev->name, &ev->bdaddr,
                ev->link_type);
 
         mask |= hci_proto_connect_ind(hdev, &ev->bdaddr, ev->link_type);
@@ -2314,6 +2401,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cc_read_local_amp_info(hdev, skb);
                 break;
 
+        case HCI_OP_READ_LOCAL_AMP_ASSOC:
+                hci_cc_read_local_amp_assoc(hdev, skb);
+                break;
+
         case HCI_OP_DELETE_STORED_LINK_KEY:
                 hci_cc_delete_stored_link_key(hdev, skb);
                 break;
@@ -2386,6 +2477,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cc_write_le_host_supported(hdev, skb);
                 break;
 
+        case HCI_OP_WRITE_REMOTE_AMP_ASSOC:
+                hci_cc_write_remote_amp_assoc(hdev, skb);
+                break;
+
         default:
                 BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
                 break;
@@ -2467,6 +2562,14 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cs_le_start_enc(hdev, ev->status);
                 break;
 
+        case HCI_OP_CREATE_PHY_LINK:
+                hci_cs_create_phylink(hdev, ev->status);
+                break;
+
+        case HCI_OP_ACCEPT_PHY_LINK:
+                hci_cs_accept_phylink(hdev, ev->status);
+                break;
+
         default:
                 BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
                 break;
@@ -2574,6 +2677,27 @@ static void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *skb)
         queue_work(hdev->workqueue, &hdev->tx_work);
 }
 
+static struct hci_conn *__hci_conn_lookup_handle(struct hci_dev *hdev,
+                                                 __u16 handle)
+{
+        struct hci_chan *chan;
+
+        switch (hdev->dev_type) {
+        case HCI_BREDR:
+                return hci_conn_hash_lookup_handle(hdev, handle);
+        case HCI_AMP:
+                chan = hci_chan_lookup_handle(hdev, handle);
+                if (chan)
+                        return chan->conn;
+                break;
+        default:
+                BT_ERR("%s unknown dev_type %d", hdev->name, hdev->dev_type);
+                break;
+        }
+
+        return NULL;
+}
+
 static void hci_num_comp_blocks_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_ev_num_comp_blocks *ev = (void *) skb->data;
@@ -2595,13 +2719,13 @@ static void hci_num_comp_blocks_evt(struct hci_dev *hdev, struct sk_buff *skb)
 
         for (i = 0; i < ev->num_hndl; i++) {
                 struct hci_comp_blocks_info *info = &ev->handles[i];
-                struct hci_conn *conn;
+                struct hci_conn *conn = NULL;
                 __u16  handle, block_count;
 
                 handle = __le16_to_cpu(info->handle);
                 block_count = __le16_to_cpu(info->blocks);
 
-                conn = hci_conn_hash_lookup_handle(hdev, handle);
+                conn = __hci_conn_lookup_handle(hdev, handle);
                 if (!conn)
                         continue;
 
@@ -2609,6 +2733,7 @@ static void hci_num_comp_blocks_evt(struct hci_dev *hdev, struct sk_buff *skb)
 
                 switch (conn->type) {
                 case ACL_LINK:
+                case AMP_LINK:
                         hdev->block_cnt += block_count;
                         if (hdev->block_cnt > hdev->num_blocks)
                                 hdev->block_cnt = hdev->num_blocks;
@@ -2705,13 +2830,13 @@ static void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
 
         key = hci_find_link_key(hdev, &ev->bdaddr);
         if (!key) {
-                BT_DBG("%s link key not found for %s", hdev->name,
-                       batostr(&ev->bdaddr));
+                BT_DBG("%s link key not found for %pMR", hdev->name,
+                       &ev->bdaddr);
                 goto not_found;
         }
 
-        BT_DBG("%s found key type %u for %s", hdev->name, key->type,
-               batostr(&ev->bdaddr));
+        BT_DBG("%s found key type %u for %pMR", hdev->name, key->type,
+               &ev->bdaddr);
 
         if (!test_bit(HCI_DEBUG_KEYS, &hdev->dev_flags) &&
             key->type == HCI_LK_DEBUG_COMBINATION) {
@@ -3558,6 +3683,22 @@ static void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb)
         }
 }
 
+static void hci_chan_selected_evt(struct hci_dev *hdev, struct sk_buff *skb)
+{
+        struct hci_ev_channel_selected *ev = (void *) skb->data;
+        struct hci_conn *hcon;
+
+        BT_DBG("%s handle 0x%2.2x", hdev->name, ev->phy_handle);
+
+        skb_pull(skb, sizeof(*ev));
+
+        hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle);
+        if (!hcon)
+                return;
+
+        amp_read_loc_assoc_final_data(hdev, hcon);
+}
+
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_event_hdr *hdr = (void *) skb->data;
@@ -3722,6 +3863,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_le_meta_evt(hdev, skb);
                 break;
 
+        case HCI_EV_CHANNEL_SELECTED:
+                hci_chan_selected_evt(hdev, skb);
+                break;
+
         case HCI_EV_REMOTE_OOB_DATA_REQUEST:
                 hci_remote_oob_data_request_evt(hdev, skb);
                 break;
diff --git a/net/bluetooth/hci_sysfs.c b/net/bluetooth/hci_sysfs.c
index a20e61c3653..55cceee02a8 100644
--- a/net/bluetooth/hci_sysfs.c
+++ b/net/bluetooth/hci_sysfs.c
@@ -38,7 +38,7 @@ static ssize_t show_link_address(struct device *dev,
                                  struct device_attribute *attr, char *buf)
 {
         struct hci_conn *conn = to_hci_conn(dev);
-        return sprintf(buf, "%s\n", batostr(&conn->dst));
+        return sprintf(buf, "%pMR\n", &conn->dst);
 }
 
 static ssize_t show_link_features(struct device *dev,
@@ -224,7 +224,7 @@ static ssize_t show_address(struct device *dev,
                             struct device_attribute *attr, char *buf)
 {
         struct hci_dev *hdev = to_hci_dev(dev);
-        return sprintf(buf, "%s\n", batostr(&hdev->bdaddr));
+        return sprintf(buf, "%pMR\n", &hdev->bdaddr);
 }
 
 static ssize_t show_features(struct device *dev,
@@ -406,8 +406,8 @@ static int inquiry_cache_show(struct seq_file *f, void *p)
 
         list_for_each_entry(e, &cache->all, all) {
                 struct inquiry_data *data = &e->data;
-                seq_printf(f, "%s %d %d %d 0x%.2x%.2x%.2x 0x%.4x %d %d %u\n",
-                           batostr(&data->bdaddr),
+                seq_printf(f, "%pMR %d %d %d 0x%.2x%.2x%.2x 0x%.4x %d %d %u\n",
+                           &data->bdaddr,
                            data->pscan_rep_mode, data->pscan_period_mode,
                            data->pscan_mode, data->dev_class[2],
                            data->dev_class[1], data->dev_class[0],
@@ -440,7 +440,7 @@ static int blacklist_show(struct seq_file *f, void *p)
         hci_dev_lock(hdev);
 
         list_for_each_entry(b, &hdev->blacklist, list)
-                seq_printf(f, "%s\n", batostr(&b->bdaddr));
+                seq_printf(f, "%pMR\n", &b->bdaddr);
 
         hci_dev_unlock(hdev);
 
diff --git a/net/bluetooth/hidp/core.c b/net/bluetooth/hidp/core.c
index ccd985da651..0c0028463fa 100644
--- a/net/bluetooth/hidp/core.c
+++ b/net/bluetooth/hidp/core.c
@@ -932,8 +932,12 @@ static int hidp_setup_hid(struct hidp_session *session,
         hid->country = req->country;
 
         strncpy(hid->name, req->name, 128);
-        strncpy(hid->phys, batostr(&bt_sk(session->ctrl_sock->sk)->src), 64);
-        strncpy(hid->uniq, batostr(&bt_sk(session->ctrl_sock->sk)->dst), 64);
+
+        snprintf(hid->phys, sizeof(hid->phys), "%pMR",
+                 &bt_sk(session->ctrl_sock->sk)->src);
+
+        snprintf(hid->uniq, sizeof(hid->uniq), "%pMR",
+                 &bt_sk(session->ctrl_sock->sk)->dst);
 
         hid->dev.parent = &session->conn->dev;
         hid->ll_driver = &hidp_hid_driver;
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index a91239dcda4..08efc256c93 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -48,19 +48,20 @@ static LIST_HEAD(chan_list);
 static DEFINE_RWLOCK(chan_list_lock);
 
 static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
-                                u8 code, u8 ident, u16 dlen, void *data);
+                                       u8 code, u8 ident, u16 dlen, void *data);
 static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
-                                                                void *data);
+                           void *data);
 static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data);
 static void l2cap_send_disconn_req(struct l2cap_conn *conn,
                                    struct l2cap_chan *chan, int err);
 
 static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
-                    struct sk_buff_head *skbs, u8 event);
+                     struct sk_buff_head *skbs, u8 event);
 
 /* ---- L2CAP channels ---- */
 
-static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid)
+static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn,
+                                                   u16 cid)
 {
         struct l2cap_chan *c;
 
@@ -71,7 +72,8 @@ static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16
         return NULL;
 }
 
-static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
+static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn,
+                                                   u16 cid)
 {
         struct l2cap_chan *c;
 
@@ -84,7 +86,8 @@ static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16
 
 /* Find channel with given SCID.
  * Returns locked channel. */
-static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid)
+static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn,
+                                                 u16 cid)
 {
         struct l2cap_chan *c;
 
@@ -97,7 +100,8 @@ static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 ci
         return c;
 }
 
-static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident)
+static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn,
+                                                    u8 ident)
 {
         struct l2cap_chan *c;
 
@@ -178,7 +182,7 @@ static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
 static void __l2cap_state_change(struct l2cap_chan *chan, int state)
 {
         BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state),
-                                                state_to_string(state));
+               state_to_string(state));
 
         chan->state = state;
         chan->ops->state_change(chan, state);
@@ -361,7 +365,7 @@ static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq)
 static void l2cap_chan_timeout(struct work_struct *work)
 {
         struct l2cap_chan *chan = container_of(work, struct l2cap_chan,
-                                                        chan_timer.work);
+                                               chan_timer.work);
         struct l2cap_conn *conn = chan->conn;
         int reason;
 
@@ -373,7 +377,7 @@ static void l2cap_chan_timeout(struct work_struct *work)
         if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG)
                 reason = ECONNREFUSED;
         else if (chan->state == BT_CONNECT &&
-                                        chan->sec_level != BT_SECURITY_SDP)
+                 chan->sec_level != BT_SECURITY_SDP)
                 reason = ECONNREFUSED;
         else
                 reason = ETIMEDOUT;
@@ -455,7 +459,7 @@ void l2cap_chan_set_defaults(struct l2cap_chan *chan)
         set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 }
 
-static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
         BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
                __le16_to_cpu(chan->psm), chan->dcid);
@@ -504,7 +508,7 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
         chan->local_msdu        = L2CAP_DEFAULT_MAX_SDU_SIZE;
         chan->local_sdu_itime   = L2CAP_DEFAULT_SDU_ITIME;
         chan->local_acc_lat     = L2CAP_DEFAULT_ACC_LAT;
-        chan->local_flush_to    = L2CAP_DEFAULT_FLUSH_TO;
+        chan->local_flush_to    = L2CAP_EFS_DEFAULT_FLUSH_TO;
 
         l2cap_chan_hold(chan);
 
@@ -527,6 +531,7 @@ void l2cap_chan_del(struct l2cap_chan *chan, int err)
         BT_DBG("chan %p, conn %p, err %d", chan, conn, err);
 
         if (conn) {
+                struct amp_mgr *mgr = conn->hcon->amp_mgr;
                 /* Delete from channel list */
                 list_del(&chan->list);
 
@@ -536,10 +541,12 @@ void l2cap_chan_del(struct l2cap_chan *chan, int err)
 
                 if (chan->chan_type != L2CAP_CHAN_CONN_FIX_A2MP)
                         hci_conn_put(conn->hcon);
+
+                if (mgr && mgr->bredr_chan == chan)
+                        mgr->bredr_chan = NULL;
         }
 
-        if (chan->ops->teardown)
-                chan->ops->teardown(chan, err);
+        chan->ops->teardown(chan, err);
 
         if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state))
                 return;
@@ -573,19 +580,18 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
         struct l2cap_conn *conn = chan->conn;
         struct sock *sk = chan->sk;
 
-        BT_DBG("chan %p state %s sk %p", chan,
-                                        state_to_string(chan->state), sk);
+        BT_DBG("chan %p state %s sk %p", chan, state_to_string(chan->state),
+               sk);
 
         switch (chan->state) {
         case BT_LISTEN:
-                if (chan->ops->teardown)
-                        chan->ops->teardown(chan, 0);
+                chan->ops->teardown(chan, 0);
                 break;
 
         case BT_CONNECTED:
         case BT_CONFIG:
                 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
-                                        conn->hcon->type == ACL_LINK) {
+                    conn->hcon->type == ACL_LINK) {
                         __set_chan_timer(chan, sk->sk_sndtimeo);
                         l2cap_send_disconn_req(conn, chan, reason);
                 } else
@@ -594,7 +600,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 
         case BT_CONNECT2:
                 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
-                                        conn->hcon->type == ACL_LINK) {
+                    conn->hcon->type == ACL_LINK) {
                         struct l2cap_conn_rsp rsp;
                         __u16 result;
 
@@ -609,7 +615,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
                         rsp.result = cpu_to_le16(result);
                         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
-                                                        sizeof(rsp), &rsp);
+                                       sizeof(rsp), &rsp);
                 }
 
                 l2cap_chan_del(chan, reason);
@@ -621,8 +627,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
                 break;
 
         default:
-                if (chan->ops->teardown)
-                        chan->ops->teardown(chan, 0);
+                chan->ops->teardown(chan, 0);
                 break;
         }
 }
@@ -691,7 +696,8 @@ static u8 l2cap_get_ident(struct l2cap_conn *conn)
         return id;
 }
 
-static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
+static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len,
+                           void *data)
 {
         struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
         u8 flags;
@@ -718,10 +724,10 @@ static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb)
         u16 flags;
 
         BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len,
-                                                        skb->priority);
+               skb->priority);
 
         if (!test_bit(FLAG_FLUSHABLE, &chan->flags) &&
-                                        lmp_no_flush_capable(hcon->hdev))
+            lmp_no_flush_capable(hcon->hdev))
                 flags = ACL_START_NO_FLUSH;
         else
                 flags = ACL_START;
@@ -946,7 +952,19 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
         return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+        struct l2cap_conn *conn = chan->conn;
+
+        if (enable_hs &&
+            chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+            conn->fixed_chan_mask & L2CAP_FC_A2MP)
+                return true;
+        else
+                return false;
+}
+
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
         struct l2cap_conn *conn = chan->conn;
         struct l2cap_conn_req req;
@@ -972,6 +990,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
         chan->ops->ready(chan);
 }
 
+static void l2cap_start_connection(struct l2cap_chan *chan)
+{
+        if (__amp_capable(chan)) {
+                BT_DBG("chan %p AMP capable: discover AMPs", chan);
+                a2mp_discover_amp(chan);
+        } else {
+                l2cap_send_conn_req(chan);
+        }
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
         struct l2cap_conn *conn = chan->conn;
@@ -986,8 +1014,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
                         return;
 
                 if (l2cap_chan_check_security(chan) &&
-                                __l2cap_no_conn_pending(chan))
-                        l2cap_send_conn_req(chan);
+                    __l2cap_no_conn_pending(chan)) {
+                        l2cap_start_connection(chan);
+                }
         } else {
                 struct l2cap_info_req req;
                 req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -997,8 +1026,8 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 
                 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
 
-                l2cap_send_cmd(conn, conn->info_ident,
-                                        L2CAP_INFO_REQ, sizeof(req), &req);
+                l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
+                               sizeof(req), &req);
         }
 }
 
@@ -1018,7 +1047,8 @@ static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
         }
 }
 
-static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err)
+static void l2cap_send_disconn_req(struct l2cap_conn *conn,
+                                   struct l2cap_chan *chan, int err)
 {
         struct sock *sk = chan->sk;
         struct l2cap_disconn_req req;
@@ -1033,14 +1063,14 @@ static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *c
         }
 
         if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
-                __l2cap_state_change(chan, BT_DISCONN);
+                l2cap_state_change(chan, BT_DISCONN);
                 return;
         }
 
         req.dcid = cpu_to_le16(chan->dcid);
         req.scid = cpu_to_le16(chan->scid);
-        l2cap_send_cmd(conn, l2cap_get_ident(conn),
-                        L2CAP_DISCONN_REQ, sizeof(req), &req);
+        l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_DISCONN_REQ,
+                       sizeof(req), &req);
 
         lock_sock(sk);
         __l2cap_state_change(chan, BT_DISCONN);
@@ -1069,20 +1099,20 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 
                 if (chan->state == BT_CONNECT) {
                         if (!l2cap_chan_check_security(chan) ||
-                                        !__l2cap_no_conn_pending(chan)) {
+                            !__l2cap_no_conn_pending(chan)) {
                                 l2cap_chan_unlock(chan);
                                 continue;
                         }
 
                         if (!l2cap_mode_supported(chan->mode, conn->feat_mask)
-                                        && test_bit(CONF_STATE2_DEVICE,
+                            && test_bit(CONF_STATE2_DEVICE,
                                         &chan->conf_state)) {
                                 l2cap_chan_close(chan, ECONNRESET);
                                 l2cap_chan_unlock(chan);
                                 continue;
                         }
 
-                        l2cap_send_conn_req(chan);
+                        l2cap_start_connection(chan);
 
                 } else if (chan->state == BT_CONNECT2) {
                         struct l2cap_conn_rsp rsp;
@@ -1094,11 +1124,9 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
                                 lock_sock(sk);
                                 if (test_bit(BT_SK_DEFER_SETUP,
                                              &bt_sk(sk)->flags)) {
-                                        struct sock *parent = bt_sk(sk)->parent;
                                         rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
                                         rsp.status = __constant_cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
-                                        if (parent)
-                                                parent->sk_data_ready(parent, 0);
+                                        chan->ops->defer(chan);
 
                                 } else {
                                         __l2cap_state_change(chan, BT_CONFIG);
@@ -1112,17 +1140,17 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
                         }
 
                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
-                                                        sizeof(rsp), &rsp);
+                                       sizeof(rsp), &rsp);
 
                         if (test_bit(CONF_REQ_SENT, &chan->conf_state) ||
-                                        rsp.result != L2CAP_CR_SUCCESS) {
+                            rsp.result != L2CAP_CR_SUCCESS) {
                                 l2cap_chan_unlock(chan);
                                 continue;
                         }
 
                         set_bit(CONF_REQ_SENT, &chan->conf_state);
                         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
-                                                l2cap_build_conf_req(chan, buf), buf);
+                                       l2cap_build_conf_req(chan, buf), buf);
                         chan->num_conf_req++;
                 }
 
@@ -1204,8 +1232,6 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn)
         bacpy(&bt_sk(sk)->src, conn->src);
         bacpy(&bt_sk(sk)->dst, conn->dst);
 
-        bt_accept_enqueue(parent, sk);
-
         l2cap_chan_add(conn, chan);
 
         l2cap_chan_ready(chan);
@@ -1270,7 +1296,7 @@ static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
 
         list_for_each_entry(chan, &conn->chan_l, list) {
                 if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags))
-                        __l2cap_chan_set_err(chan, err);
+                        l2cap_chan_set_err(chan, err);
         }
 
         mutex_unlock(&conn->chan_lock);
@@ -1279,7 +1305,7 @@ static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err)
 static void l2cap_info_timeout(struct work_struct *work)
 {
         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
-                                                        info_timer.work);
+                                               info_timer.work);
 
         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
         conn->info_ident = 0;
@@ -1333,7 +1359,7 @@ static void l2cap_conn_del(struct hci_conn *hcon, int err)
 static void security_timeout(struct work_struct *work)
 {
         struct l2cap_conn *conn = container_of(work, struct l2cap_conn,
-                                                security_timer.work);
+                                               security_timer.work);
 
         BT_DBG("conn %p", conn);
 
@@ -1355,7 +1381,7 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
         if (!hchan)
                 return NULL;
 
-        conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC);
+        conn = kzalloc(sizeof(struct l2cap_conn), GFP_KERNEL);
         if (!conn) {
                 hci_chan_del(hchan);
                 return NULL;
@@ -1367,10 +1393,22 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status)
 
         BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan);
 
-        if (hcon->hdev->le_mtu && hcon->type == LE_LINK)
-                conn->mtu = hcon->hdev->le_mtu;
-        else
+        switch (hcon->type) {
+        case AMP_LINK:
+                conn->mtu = hcon->hdev->block_mtu;
+                break;
+
+        case LE_LINK:
+                if (hcon->hdev->le_mtu) {
+                        conn->mtu = hcon->hdev->le_mtu;
+                        break;
+                }
+                /* fall through */
+
+        default:
                 conn->mtu = hcon->hdev->acl_mtu;
+                break;
+        }
 
         conn->src = &hcon->hdev->bdaddr;
         conn->dst = &hcon->dst;
@@ -1448,7 +1486,7 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
         __u8 auth_type;
         int err;
 
-        BT_DBG("%s -> %s (type %u) psm 0x%2.2x", batostr(src), batostr(dst),
+        BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", src, dst,
                dst_type, __le16_to_cpu(psm));
 
         hdev = hci_get_route(dst, src);
@@ -1461,7 +1499,7 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
 
         /* PSM must be odd and lsb of upper byte must be 0 */
         if ((__le16_to_cpu(psm) & 0x0101) != 0x0001 && !cid &&
-                                        chan->chan_type != L2CAP_CHAN_RAW) {
+            chan->chan_type != L2CAP_CHAN_RAW) {
                 err = -EINVAL;
                 goto done;
         }
@@ -1770,7 +1808,7 @@ static void l2cap_ertm_resend(struct l2cap_chan *chan)
                 skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq);
                 if (!skb) {
                         BT_DBG("Error: Can't retransmit seq %d, frame missing",
-                                seq);
+                               seq);
                         continue;
                 }
 
@@ -1795,9 +1833,9 @@ static void l2cap_ertm_resend(struct l2cap_chan *chan)
                         /* Cloned sk_buffs are read-only, so we need a
                          * writeable copy
                          */
-                        tx_skb = skb_copy(skb, GFP_ATOMIC);
+                        tx_skb = skb_copy(skb, GFP_KERNEL);
                 } else {
-                        tx_skb = skb_clone(skb, GFP_ATOMIC);
+                        tx_skb = skb_clone(skb, GFP_KERNEL);
                 }
 
                 if (!tx_skb) {
@@ -1855,7 +1893,7 @@ static void l2cap_retransmit_all(struct l2cap_chan *chan,
         if (chan->unacked_frames) {
                 skb_queue_walk(&chan->tx_q, skb) {
                         if (bt_cb(skb)->control.txseq == control->reqseq ||
-                                skb == chan->tx_send_head)
+                            skb == chan->tx_send_head)
                                 break;
                 }
 
@@ -2156,7 +2194,7 @@ static int l2cap_segment_sdu(struct l2cap_chan *chan,
 }
 
 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
-                                                                u32 priority)
+                    u32 priority)
 {
         struct sk_buff *skb;
         int err;
@@ -2543,7 +2581,7 @@ static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb)
                 /* Don't send frame to the socket it came from */
                 if (skb->sk == sk)
                         continue;
-                nskb = skb_clone(skb, GFP_ATOMIC);
+                nskb = skb_clone(skb, GFP_KERNEL);
                 if (!nskb)
                         continue;
 
@@ -2569,7 +2607,7 @@ static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code,
         len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen;
         count = min_t(unsigned int, conn->mtu, len);
 
-        skb = bt_skb_alloc(count, GFP_ATOMIC);
+        skb = bt_skb_alloc(count, GFP_KERNEL);
         if (!skb)
                 return NULL;
 
@@ -2599,7 +2637,7 @@ static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code,
         while (len) {
                 count = min_t(unsigned int, conn->mtu, len);
 
-                *frag = bt_skb_alloc(count, GFP_ATOMIC);
+                *frag = bt_skb_alloc(count, GFP_KERNEL);
                 if (!*frag)
                         goto fail;
 
@@ -2618,7 +2656,8 @@ fail:
         return NULL;
 }
 
-static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
+static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen,
+                                     unsigned long *val)
 {
         struct l2cap_conf_opt *opt = *ptr;
         int len;
@@ -2692,7 +2731,7 @@ static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
                 efs.msdu        = cpu_to_le16(chan->local_msdu);
                 efs.sdu_itime   = cpu_to_le32(chan->local_sdu_itime);
                 efs.acc_lat     = __constant_cpu_to_le32(L2CAP_DEFAULT_ACC_LAT);
-                efs.flush_to    = __constant_cpu_to_le32(L2CAP_DEFAULT_FLUSH_TO);
+                efs.flush_to    = __constant_cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO);
                 break;
 
         case L2CAP_MODE_STREAMING:
@@ -2709,7 +2748,7 @@ static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan)
         }
 
         l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs),
-                                                        (unsigned long) &efs);
+                           (unsigned long) &efs);
 }
 
 static void l2cap_ack_timeout(struct work_struct *work)
@@ -2798,13 +2837,13 @@ static inline bool __l2cap_efs_supported(struct l2cap_chan *chan)
 static inline void l2cap_txwin_setup(struct l2cap_chan *chan)
 {
         if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW &&
-                                                __l2cap_ews_supported(chan)) {
+            __l2cap_ews_supported(chan)) {
                 /* use extended control field */
                 set_bit(FLAG_EXT_CTRL, &chan->flags);
                 chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW;
         } else {
                 chan->tx_win = min_t(u16, chan->tx_win,
-                                                L2CAP_DEFAULT_TX_WINDOW);
+                                     L2CAP_DEFAULT_TX_WINDOW);
                 chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
         }
         chan->ack_win = chan->tx_win;
@@ -2844,7 +2883,7 @@ done:
         switch (chan->mode) {
         case L2CAP_MODE_BASIC:
                 if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) &&
-                                !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
+                    !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING))
                         break;
 
                 rfc.mode            = L2CAP_MODE_BASIC;
@@ -2855,7 +2894,7 @@ done:
                 rfc.max_pdu_size    = 0;
 
                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
-                                                        (unsigned long) &rfc);
+                                   (unsigned long) &rfc);
                 break;
 
         case L2CAP_MODE_ERTM:
@@ -2865,18 +2904,17 @@ done:
                 rfc.monitor_timeout = 0;
 
                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
-                                                L2CAP_EXT_HDR_SIZE -
-                                                L2CAP_SDULEN_SIZE -
-                                                L2CAP_FCS_SIZE);
+                             L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
+                             L2CAP_FCS_SIZE);
                 rfc.max_pdu_size = cpu_to_le16(size);
 
                 l2cap_txwin_setup(chan);
 
                 rfc.txwin_size = min_t(u16, chan->tx_win,
-                                                L2CAP_DEFAULT_TX_WINDOW);
+                                       L2CAP_DEFAULT_TX_WINDOW);
 
                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
-                                                        (unsigned long) &rfc);
+                                   (unsigned long) &rfc);
 
                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
                         l2cap_add_opt_efs(&ptr, chan);
@@ -2885,14 +2923,14 @@ done:
                         break;
 
                 if (chan->fcs == L2CAP_FCS_NONE ||
-                                test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
+                    test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
                         chan->fcs = L2CAP_FCS_NONE;
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
                 }
 
                 if (test_bit(FLAG_EXT_CTRL, &chan->flags))
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2,
-                                                                chan->tx_win);
+                                           chan->tx_win);
                 break;
 
         case L2CAP_MODE_STREAMING:
@@ -2904,13 +2942,12 @@ done:
                 rfc.monitor_timeout = 0;
 
                 size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu -
-                                                L2CAP_EXT_HDR_SIZE -
-                                                L2CAP_SDULEN_SIZE -
-                                                L2CAP_FCS_SIZE);
+                             L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE -
+                             L2CAP_FCS_SIZE);
                 rfc.max_pdu_size = cpu_to_le16(size);
 
                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
-                                                        (unsigned long) &rfc);
+                                   (unsigned long) &rfc);
 
                 if (test_bit(FLAG_EFS_ENABLE, &chan->flags))
                         l2cap_add_opt_efs(&ptr, chan);
@@ -2919,7 +2956,7 @@ done:
                         break;
 
                 if (chan->fcs == L2CAP_FCS_NONE ||
-                                test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
+                    test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) {
                         chan->fcs = L2CAP_FCS_NONE;
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs);
                 }
@@ -3011,7 +3048,7 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data)
         case L2CAP_MODE_ERTM:
                 if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) {
                         chan->mode = l2cap_select_mode(rfc.mode,
-                                        chan->conn->feat_mask);
+                                                       chan->conn->feat_mask);
                         break;
                 }
 
@@ -3036,8 +3073,8 @@ done:
                 if (chan->num_conf_rsp == 1)
                         return -ECONNREFUSED;
 
-                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
-                                        sizeof(rfc), (unsigned long) &rfc);
+                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
+                                   (unsigned long) &rfc);
         }
 
         if (result == L2CAP_CONF_SUCCESS) {
@@ -3054,8 +3091,8 @@ done:
 
                 if (remote_efs) {
                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
-                                        efs.stype != L2CAP_SERV_NOTRAFIC &&
-                                        efs.stype != chan->local_stype) {
+                            efs.stype != L2CAP_SERV_NOTRAFIC &&
+                            efs.stype != chan->local_stype) {
 
                                 result = L2CAP_CONF_UNACCEPT;
 
@@ -3063,8 +3100,8 @@ done:
                                         return -ECONNREFUSED;
 
                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
-                                                        sizeof(efs),
-                                                        (unsigned long) &efs);
+                                                   sizeof(efs),
+                                                   (unsigned long) &efs);
                         } else {
                                 /* Send PENDING Conf Rsp */
                                 result = L2CAP_CONF_PENDING;
@@ -3087,10 +3124,8 @@ done:
                         chan->remote_max_tx = rfc.max_transmit;
 
                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
-                                                chan->conn->mtu -
-                                                L2CAP_EXT_HDR_SIZE -
-                                                L2CAP_SDULEN_SIZE -
-                                                L2CAP_FCS_SIZE);
+                                     chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
+                                     L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
                         rfc.max_pdu_size = cpu_to_le16(size);
                         chan->remote_mps = size;
 
@@ -3102,36 +3137,35 @@ done:
                         set_bit(CONF_MODE_DONE, &chan->conf_state);
 
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
-                                        sizeof(rfc), (unsigned long) &rfc);
+                                           sizeof(rfc), (unsigned long) &rfc);
 
                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
                                 chan->remote_id = efs.id;
                                 chan->remote_stype = efs.stype;
                                 chan->remote_msdu = le16_to_cpu(efs.msdu);
                                 chan->remote_flush_to =
-                                                le32_to_cpu(efs.flush_to);
+                                        le32_to_cpu(efs.flush_to);
                                 chan->remote_acc_lat =
-                                                le32_to_cpu(efs.acc_lat);
+                                        le32_to_cpu(efs.acc_lat);
                                 chan->remote_sdu_itime =
                                         le32_to_cpu(efs.sdu_itime);
                                 l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
-                                        sizeof(efs), (unsigned long) &efs);
+                                                   sizeof(efs),
+                                                   (unsigned long) &efs);
                         }
                         break;
 
                 case L2CAP_MODE_STREAMING:
                         size = min_t(u16, le16_to_cpu(rfc.max_pdu_size),
-                                                chan->conn->mtu -
-                                                L2CAP_EXT_HDR_SIZE -
-                                                L2CAP_SDULEN_SIZE -
-                                                L2CAP_FCS_SIZE);
+                                     chan->conn->mtu - L2CAP_EXT_HDR_SIZE -
+                                     L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE);
                         rfc.max_pdu_size = cpu_to_le16(size);
                         chan->remote_mps = size;
 
                         set_bit(CONF_MODE_DONE, &chan->conf_state);
 
-                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
-                                        sizeof(rfc), (unsigned long) &rfc);
+                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc),
+                                           (unsigned long) &rfc);
 
                         break;
 
@@ -3152,7 +3186,8 @@ done:
         return ptr - data;
 }
 
-static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result)
+static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len,
+                                void *data, u16 *result)
 {
         struct l2cap_conf_req *req = data;
         void *ptr = req->data;
@@ -3179,7 +3214,7 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
                 case L2CAP_CONF_FLUSH_TO:
                         chan->flush_to = val;
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
-                                                        2, chan->flush_to);
+                                           2, chan->flush_to);
                         break;
 
                 case L2CAP_CONF_RFC:
@@ -3187,13 +3222,13 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
                                 memcpy(&rfc, (void *)val, olen);
 
                         if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) &&
-                                                        rfc.mode != chan->mode)
+                            rfc.mode != chan->mode)
                                 return -ECONNREFUSED;
 
                         chan->fcs = 0;
 
                         l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
-                                        sizeof(rfc), (unsigned long) &rfc);
+                                           sizeof(rfc), (unsigned long) &rfc);
                         break;
 
                 case L2CAP_CONF_EWS:
@@ -3207,12 +3242,12 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
                                 memcpy(&efs, (void *)val, olen);
 
                         if (chan->local_stype != L2CAP_SERV_NOTRAFIC &&
-                                        efs.stype != L2CAP_SERV_NOTRAFIC &&
-                                        efs.stype != chan->local_stype)
+                            efs.stype != L2CAP_SERV_NOTRAFIC &&
+                            efs.stype != chan->local_stype)
                                 return -ECONNREFUSED;
 
-                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS,
-                                        sizeof(efs), (unsigned long) &efs);
+                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs),
+                                           (unsigned long) &efs);
                         break;
                 }
         }
@@ -3235,10 +3270,10 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
                         if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) {
                                 chan->local_msdu = le16_to_cpu(efs.msdu);
                                 chan->local_sdu_itime =
-                                                le32_to_cpu(efs.sdu_itime);
+                                        le32_to_cpu(efs.sdu_itime);
                                 chan->local_acc_lat = le32_to_cpu(efs.acc_lat);
                                 chan->local_flush_to =
-                                                le32_to_cpu(efs.flush_to);
+                                        le32_to_cpu(efs.flush_to);
                         }
                         break;
 
@@ -3253,7 +3288,8 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi
         return ptr - data;
 }
 
-static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags)
+static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data,
+                                u16 result, u16 flags)
 {
         struct l2cap_conf_rsp *rsp = data;
         void *ptr = rsp->data;
@@ -3277,14 +3313,13 @@ void __l2cap_connect_rsp_defer(struct l2cap_chan *chan)
         rsp.dcid   = cpu_to_le16(chan->scid);
         rsp.result = __constant_cpu_to_le16(L2CAP_CR_SUCCESS);
         rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO);
-        l2cap_send_cmd(conn, chan->ident,
-                                L2CAP_CONN_RSP, sizeof(rsp), &rsp);
+        l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
 
         if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state))
                 return;
 
         l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
-                        l2cap_build_conf_req(chan, buf), buf);
+                       l2cap_build_conf_req(chan, buf), buf);
         chan->num_conf_req++;
 }
 
@@ -3339,7 +3374,8 @@ static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len)
         }
 }
 
-static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_command_rej(struct l2cap_conn *conn,
+                                    struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data;
 
@@ -3347,7 +3383,7 @@ static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hd
                 return 0;
 
         if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) &&
-                                        cmd->ident == conn->info_ident) {
+            cmd->ident == conn->info_ident) {
                 cancel_delayed_work(&conn->info_timer);
 
                 conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
@@ -3359,7 +3395,8 @@ static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hd
         return 0;
 }
 
-static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static void l2cap_connect(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd,
+                          u8 *data, u8 rsp_code, u8 amp_id)
 {
         struct l2cap_conn_req *req = (struct l2cap_conn_req *) data;
         struct l2cap_conn_rsp rsp;
@@ -3386,7 +3423,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
 
         /* Check if the ACL is secure enough (if not SDP) */
         if (psm != __constant_cpu_to_le16(L2CAP_PSM_SDP) &&
-                                !hci_conn_check_link_mode(conn->hcon)) {
+            !hci_conn_check_link_mode(conn->hcon)) {
                 conn->disc_reason = HCI_ERROR_AUTH_FAILURE;
                 result = L2CAP_CR_SEC_BLOCK;
                 goto response;
@@ -3411,8 +3448,6 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
         chan->psm  = psm;
         chan->dcid = scid;
 
-        bt_accept_enqueue(parent, sk);
-
         __l2cap_chan_add(conn, chan);
 
         dcid = chan->scid;
@@ -3427,7 +3462,7 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
                                 __l2cap_state_change(chan, BT_CONNECT2);
                                 result = L2CAP_CR_PEND;
                                 status = L2CAP_CS_AUTHOR_PEND;
-                                parent->sk_data_ready(parent, 0);
+                                chan->ops->defer(chan);
                         } else {
                                 __l2cap_state_change(chan, BT_CONFIG);
                                 result = L2CAP_CR_SUCCESS;
@@ -3453,7 +3488,7 @@ sendresp:
         rsp.dcid   = cpu_to_le16(dcid);
         rsp.result = cpu_to_le16(result);
         rsp.status = cpu_to_le16(status);
-        l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp);
+        l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp);
 
         if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) {
                 struct l2cap_info_req info;
@@ -3464,23 +3499,29 @@ sendresp:
 
                 schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT);
 
-                l2cap_send_cmd(conn, conn->info_ident,
-                                        L2CAP_INFO_REQ, sizeof(info), &info);
+                l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ,
+                               sizeof(info), &info);
         }
 
         if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) &&
-                                result == L2CAP_CR_SUCCESS) {
+            result == L2CAP_CR_SUCCESS) {
                 u8 buf[128];
                 set_bit(CONF_REQ_SENT, &chan->conf_state);
                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
-                                        l2cap_build_conf_req(chan, buf), buf);
+                               l2cap_build_conf_req(chan, buf), buf);
                 chan->num_conf_req++;
         }
+}
 
+static int l2cap_connect_req(struct l2cap_conn *conn,
+                             struct l2cap_cmd_hdr *cmd, u8 *data)
+{
+        l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0);
         return 0;
 }
 
-static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_connect_rsp(struct l2cap_conn *conn,
+                                    struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data;
         u16 scid, dcid, result, status;
@@ -3494,7 +3535,7 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
         status = __le16_to_cpu(rsp->status);
 
         BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x",
-                                                dcid, scid, result, status);
+               dcid, scid, result, status);
 
         mutex_lock(&conn->chan_lock);
 
@@ -3527,7 +3568,7 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
                         break;
 
                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
-                                        l2cap_build_conf_req(chan, req), req);
+                               l2cap_build_conf_req(chan, req), req);
                 chan->num_conf_req++;
                 break;
 
@@ -3559,7 +3600,25 @@ static inline void set_default_fcs(struct l2cap_chan *chan)
                 chan->fcs = L2CAP_FCS_CRC16;
 }
 
-static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
+static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data,
+                                    u8 ident, u16 flags)
+{
+        struct l2cap_conn *conn = chan->conn;
+
+        BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident,
+               flags);
+
+        clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
+        set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
+
+        l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP,
+                       l2cap_build_conf_rsp(chan, data,
+                                            L2CAP_CONF_SUCCESS, flags), data);
+}
+
+static inline int l2cap_config_req(struct l2cap_conn *conn,
+                                   struct l2cap_cmd_hdr *cmd, u16 cmd_len,
+                                   u8 *data)
 {
         struct l2cap_conf_req *req = (struct l2cap_conf_req *) data;
         u16 dcid, flags;
@@ -3584,7 +3643,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                 rej.dcid = cpu_to_le16(chan->dcid);
 
                 l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ,
-                                sizeof(rej), &rej);
+                               sizeof(rej), &rej);
                 goto unlock;
         }
 
@@ -3592,8 +3651,8 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         len = cmd_len - sizeof(*req);
         if (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) {
                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
-                                l2cap_build_conf_rsp(chan, rsp,
-                                        L2CAP_CONF_REJECT, flags), rsp);
+                               l2cap_build_conf_rsp(chan, rsp,
+                               L2CAP_CONF_REJECT, flags), rsp);
                 goto unlock;
         }
 
@@ -3604,8 +3663,8 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         if (flags & L2CAP_CONF_FLAG_CONTINUATION) {
                 /* Incomplete config. Send empty response. */
                 l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
-                                l2cap_build_conf_rsp(chan, rsp,
-                                        L2CAP_CONF_SUCCESS, flags), rsp);
+                               l2cap_build_conf_rsp(chan, rsp,
+                               L2CAP_CONF_SUCCESS, flags), rsp);
                 goto unlock;
         }
 
@@ -3643,23 +3702,22 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) {
                 u8 buf[64];
                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
-                                        l2cap_build_conf_req(chan, buf), buf);
+                               l2cap_build_conf_req(chan, buf), buf);
                 chan->num_conf_req++;
         }
 
         /* Got Conf Rsp PENDING from remote side and asume we sent
            Conf Rsp PENDING in the code above */
         if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) &&
-                        test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
+            test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) {
 
                 /* check compatibility */
 
-                clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
-                set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
-
-                l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
-                                        l2cap_build_conf_rsp(chan, rsp,
-                                        L2CAP_CONF_SUCCESS, flags), rsp);
+                /* Send rsp for BR/EDR channel */
+                if (!chan->ctrl_id)
+                        l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags);
+                else
+                        chan->ident = cmd->ident;
         }
 
 unlock:
@@ -3667,7 +3725,8 @@ unlock:
         return err;
 }
 
-static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_config_rsp(struct l2cap_conn *conn,
+                                   struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data;
         u16 scid, flags, result;
@@ -3699,7 +3758,7 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                         char buf[64];
 
                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
-                                                                buf, &result);
+                                                   buf, &result);
                         if (len < 0) {
                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
                                 goto done;
@@ -3707,12 +3766,11 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 
                         /* check compatibility */
 
-                        clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
-                        set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
-
-                        l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP,
-                                                l2cap_build_conf_rsp(chan, buf,
-                                                L2CAP_CONF_SUCCESS, 0x0000), buf);
+                        if (!chan->ctrl_id)
+                                l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
+                                                        0);
+                        else
+                                chan->ident = cmd->ident;
                 }
                 goto done;
 
@@ -3728,14 +3786,14 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                         /* throw out any old stored conf requests */
                         result = L2CAP_CONF_SUCCESS;
                         len = l2cap_parse_conf_rsp(chan, rsp->data, len,
-                                                                req, &result);
+                                                   req, &result);
                         if (len < 0) {
                                 l2cap_send_disconn_req(conn, chan, ECONNRESET);
                                 goto done;
                         }
 
                         l2cap_send_cmd(conn, l2cap_get_ident(conn),
-                                                L2CAP_CONF_REQ, len, req);
+                                       L2CAP_CONF_REQ, len, req);
                         chan->num_conf_req++;
                         if (result != L2CAP_CONF_SUCCESS)
                                 goto done;
@@ -3773,7 +3831,8 @@ done:
         return err;
 }
 
-static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_disconnect_req(struct l2cap_conn *conn,
+                                       struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data;
         struct l2cap_disconn_rsp rsp;
@@ -3819,7 +3878,8 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
         return 0;
 }
 
-static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn,
+                                       struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data;
         u16 dcid, scid;
@@ -3853,7 +3913,8 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
         return 0;
 }
 
-static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_information_req(struct l2cap_conn *conn,
+                                        struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_info_req *req = (struct l2cap_info_req *) data;
         u16 type;
@@ -3870,14 +3931,14 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
                 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
                 if (!disable_ertm)
                         feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
-                                                         | L2CAP_FEAT_FCS;
+                                | L2CAP_FEAT_FCS;
                 if (enable_hs)
                         feat_mask |= L2CAP_FEAT_EXT_FLOW
-                                                | L2CAP_FEAT_EXT_WINDOW;
+                                | L2CAP_FEAT_EXT_WINDOW;
 
                 put_unaligned_le32(feat_mask, rsp->data);
-                l2cap_send_cmd(conn, cmd->ident,
-                                        L2CAP_INFO_RSP, sizeof(buf), buf);
+                l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
+                               buf);
         } else if (type == L2CAP_IT_FIXED_CHAN) {
                 u8 buf[12];
                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
@@ -3890,20 +3951,21 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
                 rsp->type   = __constant_cpu_to_le16(L2CAP_IT_FIXED_CHAN);
                 rsp->result = __constant_cpu_to_le16(L2CAP_IR_SUCCESS);
                 memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan));
-                l2cap_send_cmd(conn, cmd->ident,
-                                        L2CAP_INFO_RSP, sizeof(buf), buf);
+                l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf),
+                               buf);
         } else {
                 struct l2cap_info_rsp rsp;
                 rsp.type   = cpu_to_le16(type);
                 rsp.result = __constant_cpu_to_le16(L2CAP_IR_NOTSUPP);
-                l2cap_send_cmd(conn, cmd->ident,
-                                        L2CAP_INFO_RSP, sizeof(rsp), &rsp);
+                l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp),
+                               &rsp);
         }
 
         return 0;
 }
 
-static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data)
+static inline int l2cap_information_rsp(struct l2cap_conn *conn,
+                                        struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data;
         u16 type, result;
@@ -3915,7 +3977,7 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
 
         /* L2CAP Info req/rsp are unbound to channels, add extra checks */
         if (cmd->ident != conn->info_ident ||
-                        conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
+            conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)
                 return 0;
 
         cancel_delayed_work(&conn->info_timer);
@@ -3940,7 +4002,7 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
                         conn->info_ident = l2cap_get_ident(conn);
 
                         l2cap_send_cmd(conn, conn->info_ident,
-                                        L2CAP_INFO_REQ, sizeof(req), &req);
+                                       L2CAP_INFO_REQ, sizeof(req), &req);
                 } else {
                         conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE;
                         conn->info_ident = 0;
@@ -3962,8 +4024,8 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm
 }
 
 static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
-                                        struct l2cap_cmd_hdr *cmd, u16 cmd_len,
-                                        void *data)
+                                           struct l2cap_cmd_hdr *cmd,
+                                           u16 cmd_len, void *data)
 {
         struct l2cap_create_chan_req *req = data;
         struct l2cap_create_chan_rsp rsp;
@@ -3993,7 +4055,8 @@ static inline int l2cap_create_channel_req(struct l2cap_conn *conn,
 }
 
 static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn,
-                                        struct l2cap_cmd_hdr *cmd, void *data)
+                                           struct l2cap_cmd_hdr *cmd,
+                                           void *data)
 {
         BT_DBG("conn %p", conn);
 
@@ -4126,7 +4189,7 @@ static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
 }
 
 static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
-                                                        u16 to_multiplier)
+                                         u16 to_multiplier)
 {
         u16 max_latency;
 
@@ -4147,7 +4210,8 @@ static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency,
 }
 
 static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
-                                        struct l2cap_cmd_hdr *cmd, u8 *data)
+                                              struct l2cap_cmd_hdr *cmd,
+                                              u8 *data)
 {
         struct hci_conn *hcon = conn->hcon;
         struct l2cap_conn_param_update_req *req;
@@ -4169,7 +4233,7 @@ static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
         to_multiplier   = __le16_to_cpu(req->to_multiplier);
 
         BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x",
-                                                min, max, latency, to_multiplier);
+               min, max, latency, to_multiplier);
 
         memset(&rsp, 0, sizeof(rsp));
 
@@ -4180,7 +4244,7 @@ static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
                 rsp.result = __constant_cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED);
 
         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP,
-                                                        sizeof(rsp), &rsp);
+                       sizeof(rsp), &rsp);
 
         if (!err)
                 hci_le_conn_update(hcon, min, max, latency, to_multiplier);
@@ -4189,7 +4253,8 @@ static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn,
 }
 
 static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
-                        struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data)
+                                      struct l2cap_cmd_hdr *cmd, u16 cmd_len,
+                                      u8 *data)
 {
         int err = 0;
 
@@ -4203,6 +4268,7 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
                 break;
 
         case L2CAP_CONN_RSP:
+        case L2CAP_CREATE_CHAN_RSP:
                 err = l2cap_connect_rsp(conn, cmd, data);
                 break;
 
@@ -4241,10 +4307,6 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
                 err = l2cap_create_channel_req(conn, cmd, cmd_len, data);
                 break;
 
-        case L2CAP_CREATE_CHAN_RSP:
-                err = l2cap_create_channel_rsp(conn, cmd, data);
-                break;
-
         case L2CAP_MOVE_CHAN_REQ:
                 err = l2cap_move_channel_req(conn, cmd, cmd_len, data);
                 break;
@@ -4271,7 +4333,7 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn,
 }
 
 static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
-                                        struct l2cap_cmd_hdr *cmd, u8 *data)
+                                   struct l2cap_cmd_hdr *cmd, u8 *data)
 {
         switch (cmd->code) {
         case L2CAP_COMMAND_REJ:
@@ -4290,7 +4352,7 @@ static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn,
 }
 
 static inline void l2cap_sig_channel(struct l2cap_conn *conn,
-                                                        struct sk_buff *skb)
+                                     struct sk_buff *skb)
 {
         u8 *data = skb->data;
         int len = skb->len;
@@ -4307,7 +4369,8 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn,
 
                 cmd_len = le16_to_cpu(cmd.len);
 
-                BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident);
+                BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len,
+                       cmd.ident);
 
                 if (cmd_len > len || !cmd.ident) {
                         BT_DBG("corrupted command");
@@ -4326,7 +4389,8 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn,
 
                         /* FIXME: Map err to a valid reason */
                         rej.reason = __constant_cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD);
-                        l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej);
+                        l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ,
+                                       sizeof(rej), &rej);
                 }
 
                 data += cmd_len;
@@ -4391,8 +4455,8 @@ static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan)
         }
 }
 
-static void append_skb_frag(struct sk_buff *skb,
-                        struct sk_buff *new_frag, struct sk_buff **last_frag)
+static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag,
+                            struct sk_buff **last_frag)
 {
         /* skb->len reflects data in skb as well as all fragments
          * skb->data_len reflects only data in fragments
@@ -4641,7 +4705,7 @@ static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
 
         if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) {
                 if (__seq_offset(chan, txseq, chan->last_acked_seq) >=
-                                                                chan->tx_win) {
+                    chan->tx_win) {
                         /* See notes below regarding "double poll" and
                          * invalid packets.
                          */
@@ -4682,8 +4746,7 @@ static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq)
         }
 
         if (__seq_offset(chan, txseq, chan->last_acked_seq) <
-                __seq_offset(chan, chan->expected_tx_seq,
-                             chan->last_acked_seq)){
+            __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) {
                 BT_DBG("Duplicate - expected_tx_seq later than txseq");
                 return L2CAP_TXSEQ_DUPLICATE;
         }
@@ -5323,7 +5386,7 @@ int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
         int exact = 0, lm1 = 0, lm2 = 0;
         struct l2cap_chan *c;
 
-        BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
+        BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
 
         /* Find listening sockets and check their link_mode */
         read_lock(&chan_list_lock);
@@ -5353,7 +5416,7 @@ void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
 {
         struct l2cap_conn *conn;
 
-        BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
+        BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
 
         if (!status) {
                 conn = l2cap_conn_add(hcon, status);
@@ -5443,7 +5506,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
                 }
 
                 if (!status && (chan->state == BT_CONNECTED ||
-                                                chan->state == BT_CONFIG)) {
+                                chan->state == BT_CONFIG)) {
                         struct sock *sk = chan->sk;
 
                         clear_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags);
@@ -5456,7 +5519,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
                 if (chan->state == BT_CONNECT) {
                         if (!status) {
-                                l2cap_send_conn_req(chan);
+                                l2cap_start_connection(chan);
                         } else {
                                 __set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
                         }
@@ -5470,11 +5533,9 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
                         if (!status) {
                                 if (test_bit(BT_SK_DEFER_SETUP,
                                              &bt_sk(sk)->flags)) {
-                                        struct sock *parent = bt_sk(sk)->parent;
                                         res = L2CAP_CR_PEND;
                                         stat = L2CAP_CS_AUTHOR_PEND;
-                                        if (parent)
-                                                parent->sk_data_ready(parent, 0);
+                                        chan->ops->defer(chan);
                                 } else {
                                         __l2cap_state_change(chan, BT_CONFIG);
                                         res = L2CAP_CR_SUCCESS;
@@ -5494,7 +5555,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
                         rsp.result = cpu_to_le16(res);
                         rsp.status = cpu_to_le16(stat);
                         l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP,
-                                                        sizeof(rsp), &rsp);
+                                       sizeof(rsp), &rsp);
 
                         if (!test_bit(CONF_REQ_SENT, &chan->conf_state) &&
                             res == L2CAP_CR_SUCCESS) {
@@ -5519,6 +5580,12 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
 {
         struct l2cap_conn *conn = hcon->l2cap_data;
+        struct l2cap_hdr *hdr;
+        int len;
+
+        /* For AMP controller do not create l2cap conn */
+        if (!conn && hcon->hdev->dev_type != HCI_BREDR)
+                goto drop;
 
         if (!conn)
                 conn = l2cap_conn_add(hcon, 0);
@@ -5528,10 +5595,10 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
 
         BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
 
-        if (!(flags & ACL_CONT)) {
-                struct l2cap_hdr *hdr;
-                int len;
-
+        switch (flags) {
+        case ACL_START:
+        case ACL_START_NO_FLUSH:
+        case ACL_COMPLETE:
                 if (conn->rx_len) {
                         BT_ERR("Unexpected start frame (len %d)", skb->len);
                         kfree_skb(conn->rx_skb);
@@ -5560,20 +5627,22 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
 
                 if (skb->len > len) {
                         BT_ERR("Frame is too long (len %d, expected len %d)",
-                                skb->len, len);
+                               skb->len, len);
                         l2cap_conn_unreliable(conn, ECOMM);
                         goto drop;
                 }
 
                 /* Allocate skb for the complete frame (with header) */
-                conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC);
+                conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL);
                 if (!conn->rx_skb)
                         goto drop;
 
                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
-                                                                skb->len);
+                                          skb->len);
                 conn->rx_len = len - skb->len;
-        } else {
+                break;
+
+        case ACL_CONT:
                 BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len);
 
                 if (!conn->rx_len) {
@@ -5584,7 +5653,7 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
 
                 if (skb->len > conn->rx_len) {
                         BT_ERR("Fragment is too long (len %d, expected %d)",
-                                        skb->len, conn->rx_len);
+                               skb->len, conn->rx_len);
                         kfree_skb(conn->rx_skb);
                         conn->rx_skb = NULL;
                         conn->rx_len = 0;
@@ -5593,7 +5662,7 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
                 }
 
                 skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len),
-                                                                skb->len);
+                                          skb->len);
                 conn->rx_len -= skb->len;
 
                 if (!conn->rx_len) {
@@ -5601,6 +5670,7 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
                         l2cap_recv_frame(conn, conn->rx_skb);
                         conn->rx_skb = NULL;
                 }
+                break;
         }
 
 drop:
@@ -5617,12 +5687,11 @@ static int l2cap_debugfs_show(struct seq_file *f, void *p)
         list_for_each_entry(c, &chan_list, global_l) {
                 struct sock *sk = c->sk;
 
-                seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
-                                        batostr(&bt_sk(sk)->src),
-                                        batostr(&bt_sk(sk)->dst),
-                                        c->state, __le16_to_cpu(c->psm),
-                                        c->scid, c->dcid, c->imtu, c->omtu,
-                                        c->sec_level, c->mode);
+                seq_printf(f, "%pMR %pMR %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
+                           &bt_sk(sk)->src, &bt_sk(sk)->dst,
+                           c->state, __le16_to_cpu(c->psm),
+                           c->scid, c->dcid, c->imtu, c->omtu,
+                           c->sec_level, c->mode);
         }
 
         read_unlock(&chan_list_lock);
@@ -5653,8 +5722,8 @@ int __init l2cap_init(void)
                 return err;
 
         if (bt_debugfs) {
-                l2cap_debugfs = debugfs_create_file("l2cap", 0444,
-                                        bt_debugfs, NULL, &l2cap_debugfs_fops);
+                l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
+                                                    NULL, &l2cap_debugfs_fops);
                 if (!l2cap_debugfs)
                         BT_ERR("Failed to create L2CAP debug file");
         }
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index 083f2bf065d..89f1472939e 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -40,7 +40,8 @@ static struct bt_sock_list l2cap_sk_list = {
 
 static const struct proto_ops l2cap_sock_ops;
 static void l2cap_sock_init(struct sock *sk, struct sock *parent);
-static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio);
+static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock,
+                                     int proto, gfp_t prio);
 
 static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
 {
@@ -106,7 +107,8 @@ done:
         return err;
 }
 
-static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags)
+static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr,
+                              int alen, int flags)
 {
         struct sock *sk = sock->sk;
         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -134,7 +136,7 @@ static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int al
         lock_sock(sk);
 
         err = bt_sock_wait_state(sk, BT_CONNECTED,
-                        sock_sndtimeo(sk, flags & O_NONBLOCK));
+                                 sock_sndtimeo(sk, flags & O_NONBLOCK));
 
         release_sock(sk);
 
@@ -185,7 +187,8 @@ done:
         return err;
 }
 
-static int l2cap_sock_accept(struct socket *sock, struct socket *newsock, int flags)
+static int l2cap_sock_accept(struct socket *sock, struct socket *newsock,
+                             int flags)
 {
         DECLARE_WAITQUEUE(wait, current);
         struct sock *sk = sock->sk, *nsk;
@@ -241,7 +244,8 @@ done:
         return err;
 }
 
-static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer)
+static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr,
+                              int *len, int peer)
 {
         struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr;
         struct sock *sk = sock->sk;
@@ -266,7 +270,8 @@ static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *l
         return 0;
 }
 
-static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen)
+static int l2cap_sock_getsockopt_old(struct socket *sock, int optname,
+                                     char __user *optval, int __user *optlen)
 {
         struct sock *sk = sock->sk;
         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -309,7 +314,7 @@ static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __us
                         break;
                 case BT_SECURITY_HIGH:
                         opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT |
-                                                        L2CAP_LM_SECURE;
+                              L2CAP_LM_SECURE;
                         break;
                 default:
                         opt = 0;
@@ -353,7 +358,8 @@ static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __us
         return err;
 }
 
-static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen)
+static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname,
+                                 char __user *optval, int __user *optlen)
 {
         struct sock *sk = sock->sk;
         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -377,19 +383,20 @@ static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, ch
         switch (optname) {
         case BT_SECURITY:
                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED &&
-                                        chan->chan_type != L2CAP_CHAN_RAW) {
+                    chan->chan_type != L2CAP_CHAN_RAW) {
                         err = -EINVAL;
                         break;
                 }
 
                 memset(&sec, 0, sizeof(sec));
-                if (chan->conn)
+                if (chan->conn) {
                         sec.level = chan->conn->hcon->sec_level;
-                else
-                        sec.level = chan->sec_level;
 
-                if (sk->sk_state == BT_CONNECTED)
-                        sec.key_size = chan->conn->hcon->enc_key_size;
+                        if (sk->sk_state == BT_CONNECTED)
+                                sec.key_size = chan->conn->hcon->enc_key_size;
+                } else {
+                        sec.level = chan->sec_level;
+                }
 
                 len = min_t(unsigned int, len, sizeof(sec));
                 if (copy_to_user(optval, (char *) &sec, len))
@@ -411,14 +418,14 @@ static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, ch
 
         case BT_FLUSHABLE:
                 if (put_user(test_bit(FLAG_FLUSHABLE, &chan->flags),
-                                                (u32 __user *) optval))
+                             (u32 __user *) optval))
                         err = -EFAULT;
 
                 break;
 
         case BT_POWER:
                 if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM
-                                && sk->sk_type != SOCK_RAW) {
+                    && sk->sk_type != SOCK_RAW) {
                         err = -EINVAL;
                         break;
                 }
@@ -466,7 +473,8 @@ static bool l2cap_valid_mtu(struct l2cap_chan *chan, u16 mtu)
         return true;
 }
 
-static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __user *optval, unsigned int optlen)
+static int l2cap_sock_setsockopt_old(struct socket *sock, int optname,
+                                     char __user *optval, unsigned int optlen)
 {
         struct sock *sk = sock->sk;
         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -529,6 +537,7 @@ static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __us
                 chan->fcs  = opts.fcs;
                 chan->max_tx = opts.max_tx;
                 chan->tx_win = opts.txwin_size;
+                chan->flush_to = opts.flush_to;
                 break;
 
         case L2CAP_LM:
@@ -564,7 +573,8 @@ static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __us
         return err;
 }
 
-static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int optlen)
+static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname,
+                                 char __user *optval, unsigned int optlen)
 {
         struct sock *sk = sock->sk;
         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -587,7 +597,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
         switch (optname) {
         case BT_SECURITY:
                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED &&
-                                        chan->chan_type != L2CAP_CHAN_RAW) {
+                    chan->chan_type != L2CAP_CHAN_RAW) {
                         err = -EINVAL;
                         break;
                 }
@@ -601,7 +611,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
                 }
 
                 if (sec.level < BT_SECURITY_LOW ||
-                                        sec.level > BT_SECURITY_HIGH) {
+                    sec.level > BT_SECURITY_HIGH) {
                         err = -EINVAL;
                         break;
                 }
@@ -627,7 +637,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
 
                 /* or for ACL link */
                 } else if ((sk->sk_state == BT_CONNECT2 &&
-                           test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) ||
+                            test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) ||
                            sk->sk_state == BT_CONNECTED) {
                         if (!l2cap_chan_check_security(chan))
                                 set_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags);
@@ -684,7 +694,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
 
         case BT_POWER:
                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED &&
-                                        chan->chan_type != L2CAP_CHAN_RAW) {
+                    chan->chan_type != L2CAP_CHAN_RAW) {
                         err = -EINVAL;
                         break;
                 }
@@ -720,7 +730,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
                 }
 
                 if (chan->mode != L2CAP_MODE_ERTM &&
-                                chan->mode != L2CAP_MODE_STREAMING) {
+                    chan->mode != L2CAP_MODE_STREAMING) {
                         err = -EOPNOTSUPP;
                         break;
                 }
@@ -737,7 +747,8 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, ch
         return err;
 }
 
-static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
+static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
+                              struct msghdr *msg, size_t len)
 {
         struct sock *sk = sock->sk;
         struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -762,7 +773,8 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
         return err;
 }
 
-static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len, int flags)
+static int l2cap_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
+                              struct msghdr *msg, size_t len, int flags)
 {
         struct sock *sk = sock->sk;
         struct l2cap_pinfo *pi = l2cap_pi(sk);
@@ -866,7 +878,7 @@ static int l2cap_sock_shutdown(struct socket *sock, int how)
 
                 if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
                         err = bt_sock_wait_state(sk, BT_CLOSED,
-                                                        sk->sk_lingertime);
+                                                 sk->sk_lingertime);
         }
 
         if (!err && sk->sk_err)
@@ -930,7 +942,7 @@ static struct l2cap_chan *l2cap_sock_new_connection_cb(struct l2cap_chan *chan)
         }
 
         sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP,
-                                                                GFP_ATOMIC);
+                              GFP_ATOMIC);
         if (!sk)
                 return NULL;
 
@@ -938,6 +950,8 @@ static struct l2cap_chan *l2cap_sock_new_connection_cb(struct l2cap_chan *chan)
 
         l2cap_sock_init(sk, parent);
 
+        bt_accept_enqueue(parent, sk);
+
         return l2cap_pi(sk)->chan;
 }
 
@@ -1068,6 +1082,15 @@ static void l2cap_sock_ready_cb(struct l2cap_chan *chan)
         release_sock(sk);
 }
 
+static void l2cap_sock_defer_cb(struct l2cap_chan *chan)
+{
+        struct sock *sk = chan->data;
+        struct sock *parent = bt_sk(sk)->parent;
+
+        if (parent)
+                parent->sk_data_ready(parent, 0);
+}
+
 static struct l2cap_ops l2cap_chan_ops = {
         .name           = "L2CAP Socket Interface",
         .new_connection = l2cap_sock_new_connection_cb,
@@ -1076,6 +1099,7 @@ static struct l2cap_ops l2cap_chan_ops = {
         .teardown       = l2cap_sock_teardown_cb,
         .state_change   = l2cap_sock_state_change_cb,
         .ready          = l2cap_sock_ready_cb,
+        .defer          = l2cap_sock_defer_cb,
         .alloc_skb      = l2cap_sock_alloc_skb_cb,
 };
 
@@ -1083,7 +1107,8 @@ static void l2cap_sock_destruct(struct sock *sk)
 {
         BT_DBG("sk %p", sk);
 
-        l2cap_chan_put(l2cap_pi(sk)->chan);
+        if (l2cap_pi(sk)->chan)
+                l2cap_chan_put(l2cap_pi(sk)->chan);
         if (l2cap_pi(sk)->rx_busy_skb) {
                 kfree_skb(l2cap_pi(sk)->rx_busy_skb);
                 l2cap_pi(sk)->rx_busy_skb = NULL;
@@ -1159,7 +1184,8 @@ static struct proto l2cap_proto = {
         .obj_size       = sizeof(struct l2cap_pinfo)
 };
 
-static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio)
+static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock,
+                                     int proto, gfp_t prio)
 {
         struct sock *sk;
         struct l2cap_chan *chan;
@@ -1204,7 +1230,7 @@ static int l2cap_sock_create(struct net *net, struct socket *sock, int protocol,
         sock->state = SS_UNCONNECTED;
 
         if (sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM &&
-                        sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
+            sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
                 return -ESOCKTNOSUPPORT;
 
         if (sock->type == SOCK_RAW && !kern && !capable(CAP_NET_RAW))
@@ -1261,7 +1287,8 @@ int __init l2cap_init_sockets(void)
                 goto error;
         }
 
-        err = bt_procfs_init(THIS_MODULE, &init_net, "l2cap", &l2cap_sk_list, NULL);
+        err = bt_procfs_init(THIS_MODULE, &init_net, "l2cap", &l2cap_sk_list,
+                             NULL);
         if (err < 0) {
                 BT_ERR("Failed to create L2CAP proc file");
                 bt_sock_unregister(BTPROTO_L2CAP);
diff --git a/net/bluetooth/lib.c b/net/bluetooth/lib.c
index e1c97527e16..b3fbc73516c 100644
--- a/net/bluetooth/lib.c
+++ b/net/bluetooth/lib.c
@@ -41,20 +41,6 @@ void baswap(bdaddr_t *dst, bdaddr_t *src)
 }
 EXPORT_SYMBOL(baswap);
 
-char *batostr(bdaddr_t *ba)
-{
-        static char str[2][18];
-        static int i = 1;
-
-        i ^= 1;
-        sprintf(str[i], "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
-                ba->b[5], ba->b[4], ba->b[3],
-                ba->b[2], ba->b[1], ba->b[0]);
-
-        return str[i];
-}
-EXPORT_SYMBOL(batostr);
-
 /* Bluetooth error codes to Unix errno mapping */
 int bt_to_errno(__u16 code)
 {
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index aa2ea0a8142..399e5024b5b 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -3125,6 +3125,9 @@ int mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr,
         struct pending_cmd *cmd;
         int err;
 
+        mgmt_pending_foreach(MGMT_OP_UNPAIR_DEVICE, hdev, unpair_device_rsp,
+                             hdev);
+
         cmd = mgmt_pending_find(MGMT_OP_DISCONNECT, hdev);
         if (!cmd)
                 return -ENOENT;
@@ -3137,8 +3140,6 @@ int mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr,
 
         mgmt_pending_remove(cmd);
 
-        mgmt_pending_foreach(MGMT_OP_UNPAIR_DEVICE, hdev, unpair_device_rsp,
-                             hdev);
         return err;
 }
 
diff --git a/net/bluetooth/rfcomm/core.c b/net/bluetooth/rfcomm/core.c
index c75107ef892..201fdf73720 100644
--- a/net/bluetooth/rfcomm/core.c
+++ b/net/bluetooth/rfcomm/core.c
@@ -377,8 +377,8 @@ static int __rfcomm_dlc_open(struct rfcomm_dlc *d, bdaddr_t *src, bdaddr_t *dst,
         int err = 0;
         u8 dlci;
 
-        BT_DBG("dlc %p state %ld %s %s channel %d",
-                        d, d->state, batostr(src), batostr(dst), channel);
+        BT_DBG("dlc %p state %ld %pMR -> %pMR channel %d",
+               d, d->state, src, dst, channel);
 
         if (channel < 1 || channel > 30)
                 return -EINVAL;
@@ -676,7 +676,7 @@ static struct rfcomm_session *rfcomm_session_create(bdaddr_t *src,
         struct socket *sock;
         struct sock *sk;
 
-        BT_DBG("%s %s", batostr(src), batostr(dst));
+        BT_DBG("%pMR -> %pMR", src, dst);
 
         *err = rfcomm_l2sock_create(&sock);
         if (*err < 0)
@@ -709,7 +709,7 @@ static struct rfcomm_session *rfcomm_session_create(bdaddr_t *src,
 
         bacpy(&addr.l2_bdaddr, dst);
         addr.l2_family = AF_BLUETOOTH;
-        addr.l2_psm    = cpu_to_le16(RFCOMM_PSM);
+        addr.l2_psm    = __constant_cpu_to_le16(RFCOMM_PSM);
         addr.l2_cid    = 0;
         *err = kernel_connect(sock, (struct sockaddr *) &addr, sizeof(addr), O_NONBLOCK);
         if (*err == 0 || *err == -EINPROGRESS)
@@ -1987,7 +1987,7 @@ static int rfcomm_add_listener(bdaddr_t *ba)
         /* Bind socket */
         bacpy(&addr.l2_bdaddr, ba);
         addr.l2_family = AF_BLUETOOTH;
-        addr.l2_psm    = cpu_to_le16(RFCOMM_PSM);
+        addr.l2_psm    = __constant_cpu_to_le16(RFCOMM_PSM);
         addr.l2_cid    = 0;
         err = kernel_bind(sock, (struct sockaddr *) &addr, sizeof(addr));
         if (err < 0) {
@@ -2125,11 +2125,10 @@ static int rfcomm_dlc_debugfs_show(struct seq_file *f, void *x)
                 list_for_each_entry(d, &s->dlcs, list) {
                         struct sock *sk = s->sock->sk;
 
-                        seq_printf(f, "%s %s %ld %d %d %d %d\n",
-                                                batostr(&bt_sk(sk)->src),
-                                                batostr(&bt_sk(sk)->dst),
-                                                d->state, d->dlci, d->mtu,
-                                                d->rx_credits, d->tx_credits);
+                        seq_printf(f, "%pMR %pMR %ld %d %d %d %d\n",
+                                   &bt_sk(sk)->src, &bt_sk(sk)->dst,
+                                   d->state, d->dlci, d->mtu,
+                                   d->rx_credits, d->tx_credits);
                 }
         }
 
diff --git a/net/bluetooth/rfcomm/sock.c b/net/bluetooth/rfcomm/sock.c
index b3226f3658c..4ddef57d03a 100644
--- a/net/bluetooth/rfcomm/sock.c
+++ b/net/bluetooth/rfcomm/sock.c
@@ -334,7 +334,7 @@ static int rfcomm_sock_bind(struct socket *sock, struct sockaddr *addr, int addr
         struct sock *sk = sock->sk;
         int err = 0;
 
-        BT_DBG("sk %p %s", sk, batostr(&sa->rc_bdaddr));
+        BT_DBG("sk %p %pMR", sk, &sa->rc_bdaddr);
 
         if (!addr || addr->sa_family != AF_BLUETOOTH)
                 return -EINVAL;
@@ -975,10 +975,9 @@ static int rfcomm_sock_debugfs_show(struct seq_file *f, void *p)
         read_lock(&rfcomm_sk_list.lock);
 
         sk_for_each(sk, node, &rfcomm_sk_list.head) {
-                seq_printf(f, "%s %s %d %d\n",
-                                batostr(&bt_sk(sk)->src),
-                                batostr(&bt_sk(sk)->dst),
-                                sk->sk_state, rfcomm_pi(sk)->channel);
+                seq_printf(f, "%pMR %pMR %d %d\n",
+                           &bt_sk(sk)->src, &bt_sk(sk)->dst,
+                           sk->sk_state, rfcomm_pi(sk)->channel);
         }
 
         read_unlock(&rfcomm_sk_list.lock);
diff --git a/net/bluetooth/rfcomm/tty.c b/net/bluetooth/rfcomm/tty.c
index ccc248791d5..bd6fd0f43d2 100644
--- a/net/bluetooth/rfcomm/tty.c
+++ b/net/bluetooth/rfcomm/tty.c
@@ -166,7 +166,7 @@ static struct device *rfcomm_get_device(struct rfcomm_dev *dev)
 static ssize_t show_address(struct device *tty_dev, struct device_attribute *attr, char *buf)
 {
         struct rfcomm_dev *dev = dev_get_drvdata(tty_dev);
-        return sprintf(buf, "%s\n", batostr(&dev->dst));
+        return sprintf(buf, "%pMR\n", &dev->dst);
 }
 
 static ssize_t show_channel(struct device *tty_dev, struct device_attribute *attr, char *buf)
@@ -663,8 +663,8 @@ static int rfcomm_tty_open(struct tty_struct *tty, struct file *filp)
         if (!dev)
                 return -ENODEV;
 
-        BT_DBG("dev %p dst %s channel %d opened %d", dev, batostr(&dev->dst),
-                                dev->channel, dev->port.count);
+        BT_DBG("dev %p dst %pMR channel %d opened %d", dev, &dev->dst,
+               dev->channel, dev->port.count);
 
         spin_lock_irqsave(&dev->port.lock, flags);
         if (++dev->port.count > 1) {
diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c
index dc42b917aaa..450cdcd88e5 100644
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -172,7 +172,7 @@ static int sco_connect(struct sock *sk)
         struct hci_dev  *hdev;
         int err, type;
 
-        BT_DBG("%s -> %s", batostr(src), batostr(dst));
+        BT_DBG("%pMR -> %pMR", src, dst);
 
         hdev = hci_get_route(dst, src);
         if (!hdev)
@@ -460,7 +460,7 @@ static int sco_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_le
         struct sock *sk = sock->sk;
         int err = 0;
 
-        BT_DBG("sk %p %s", sk, batostr(&sa->sco_bdaddr));
+        BT_DBG("sk %p %pMR", sk, &sa->sco_bdaddr);
 
         if (!addr || addr->sa_family != AF_BLUETOOTH)
                 return -EINVAL;
@@ -893,7 +893,7 @@ int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
         struct hlist_node *node;
         int lm = 0;
 
-        BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr));
+        BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr);
 
         /* Find listening sockets */
         read_lock(&sco_sk_list.lock);
@@ -914,7 +914,7 @@ int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
 
 void sco_connect_cfm(struct hci_conn *hcon, __u8 status)
 {
-        BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
+        BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status);
         if (!status) {
                 struct sco_conn *conn;
 
@@ -959,8 +959,8 @@ static int sco_debugfs_show(struct seq_file *f, void *p)
         read_lock(&sco_sk_list.lock);
 
         sk_for_each(sk, node, &sco_sk_list.head) {
-                seq_printf(f, "%s %s %d\n", batostr(&bt_sk(sk)->src),
-                           batostr(&bt_sk(sk)->dst), sk->sk_state);
+                seq_printf(f, "%pMR %pMR %d\n", &bt_sk(sk)->src,
+                           &bt_sk(sk)->dst, sk->sk_state);
         }
 
         read_unlock(&sco_sk_list.lock);
diff --git a/net/bluetooth/smp.c b/net/bluetooth/smp.c
index 8c225ef349c..9176bc17595 100644
--- a/net/bluetooth/smp.c
+++ b/net/bluetooth/smp.c
@@ -32,6 +32,8 @@
 
 #define SMP_TIMEOUT     msecs_to_jiffies(30000)
 
+#define AUTH_REQ_MASK   0x07
+
 static inline void swap128(u8 src[16], u8 dst[16])
 {
         int i;
@@ -165,7 +167,7 @@ static struct sk_buff *smp_build_cmd(struct l2cap_conn *conn, u8 code,
 
         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
         lh->len = cpu_to_le16(sizeof(code) + dlen);
-        lh->cid = cpu_to_le16(L2CAP_CID_SMP);
+        lh->cid = __constant_cpu_to_le16(L2CAP_CID_SMP);
 
         memcpy(skb_put(skb, sizeof(code)), &code, sizeof(code));
 
@@ -230,7 +232,7 @@ static void build_pairing_cmd(struct l2cap_conn *conn,
                 req->max_key_size = SMP_MAX_ENC_KEY_SIZE;
                 req->init_key_dist = 0;
                 req->resp_key_dist = dist_keys;
-                req->auth_req = authreq;
+                req->auth_req = (authreq & AUTH_REQ_MASK);
                 return;
         }
 
@@ -239,7 +241,7 @@ static void build_pairing_cmd(struct l2cap_conn *conn,
         rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE;
         rsp->init_key_dist = 0;
         rsp->resp_key_dist = req->resp_key_dist & dist_keys;
-        rsp->auth_req = authreq;
+        rsp->auth_req = (authreq & AUTH_REQ_MASK);
 }
 
 static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
diff --git a/net/mac80211/ibss.c b/net/mac80211/ibss.c
index 3d5332e367f..c7386b2b767 100644
--- a/net/mac80211/ibss.c
+++ b/net/mac80211/ibss.c
@@ -1110,7 +1110,7 @@ int ieee80211_ibss_join(struct ieee80211_sub_if_data *sdata,
         sdata->u.ibss.state = IEEE80211_IBSS_MLME_SEARCH;
         sdata->u.ibss.ibss_join_req = jiffies;
 
-        memcpy(sdata->u.ibss.ssid, params->ssid, IEEE80211_MAX_SSID_LEN);
+        memcpy(sdata->u.ibss.ssid, params->ssid, params->ssid_len);
         sdata->u.ibss.ssid_len = params->ssid_len;
 
         mutex_unlock(&sdata->u.ibss.mtx);
diff --git a/net/mac80211/iface.c b/net/mac80211/iface.c
index 944c6cf53eb..1a6fe135f20 100644
--- a/net/mac80211/iface.c
+++ b/net/mac80211/iface.c
@@ -840,7 +840,7 @@ static void ieee80211_do_stop(struct ieee80211_sub_if_data *sdata,
                         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
                         if (info->control.vif == &sdata->vif) {
                                 __skb_unlink(skb, &local->pending[i]);
-                                dev_kfree_skb_irq(skb);
+                                ieee80211_free_txskb(&local->hw, skb);
                         }
                 }
         }
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index 2bdf7769506..1d1fdf0791f 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -3177,26 +3177,37 @@ static int ieee80211_prep_channel(struct ieee80211_sub_if_data *sdata,
                                    ht_cfreq, ht_oper->primary_chan,
                                    cbss->channel->band);
                         ht_oper = NULL;
+                } else {
+                        channel_type = NL80211_CHAN_HT20;
                 }
         }
 
-        if (ht_oper) {
+        if (ht_oper && sband->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40) {
+                /*
+                 * cfg80211 already verified that the channel itself can
+                 * be used, but it didn't check that we can do the right
+                 * HT type, so do that here as well. If HT40 isn't allowed
+                 * on this channel, disable 40 MHz operation.
+                 */
                 const u8 *ht_cap_ie;
                 const struct ieee80211_ht_cap *ht_cap;
                 u8 chains = 1;
 
                 channel_type = NL80211_CHAN_HT20;
 
-                if (sband->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40) {
-                        switch (ht_oper->ht_param &
-                                        IEEE80211_HT_PARAM_CHA_SEC_OFFSET) {
-                        case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
+                switch (ht_oper->ht_param & IEEE80211_HT_PARAM_CHA_SEC_OFFSET) {
+                case IEEE80211_HT_PARAM_CHA_SEC_ABOVE:
+                        if (cbss->channel->flags & IEEE80211_CHAN_NO_HT40PLUS)
+                                ifmgd->flags |= IEEE80211_STA_DISABLE_40MHZ;
+                        else
                                 channel_type = NL80211_CHAN_HT40PLUS;
-                                break;
-                        case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
+                        break;
+                case IEEE80211_HT_PARAM_CHA_SEC_BELOW:
+                        if (cbss->channel->flags & IEEE80211_CHAN_NO_HT40MINUS)
+                                ifmgd->flags |= IEEE80211_STA_DISABLE_40MHZ;
+                        else
                                 channel_type = NL80211_CHAN_HT40MINUS;
-                                break;
-                        }
+                        break;
                 }
 
                 ht_cap_ie = cfg80211_find_ie(WLAN_EID_HT_CAPABILITY,
@@ -3648,6 +3659,7 @@ int ieee80211_mgd_deauth(struct ieee80211_sub_if_data *sdata,
 {
         struct ieee80211_if_managed *ifmgd = &sdata->u.mgd;
         u8 frame_buf[IEEE80211_DEAUTH_FRAME_LEN];
+        bool tx = !req->local_state_change;
 
         mutex_lock(&ifmgd->mtx);
 
@@ -3664,12 +3676,12 @@ int ieee80211_mgd_deauth(struct ieee80211_sub_if_data *sdata,
         if (ifmgd->associated &&
             ether_addr_equal(ifmgd->associated->bssid, req->bssid)) {
                 ieee80211_set_disassoc(sdata, IEEE80211_STYPE_DEAUTH,
-                                       req->reason_code, true, frame_buf);
+                                       req->reason_code, tx, frame_buf);
         } else {
                 drv_mgd_prepare_tx(sdata->local, sdata);
                 ieee80211_send_deauth_disassoc(sdata, req->bssid,
                                                IEEE80211_STYPE_DEAUTH,
-                                               req->reason_code, true,
+                                               req->reason_code, tx,
                                                frame_buf);
         }
 
diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index c0a1f53e68a..38b382682ca 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -525,6 +525,11 @@ static ieee80211_rx_result ieee80211_rx_mesh_check(struct ieee80211_rx_data *rx)
 
                 if (ieee80211_is_action(hdr->frame_control)) {
                         u8 category;
+
+                        /* make sure category field is present */
+                        if (rx->skb->len < IEEE80211_MIN_ACTION_SIZE)
+                                return RX_DROP_MONITOR;
+
                         mgmt = (struct ieee80211_mgmt *)hdr;
                         category = mgmt->u.action.category;
                         if (category != WLAN_CATEGORY_MESH_ACTION &&
@@ -875,14 +880,16 @@ ieee80211_rx_h_check(struct ieee80211_rx_data *rx)
                  */
                 if (rx->sta && rx->sdata->vif.type == NL80211_IFTYPE_STATION &&
                     ieee80211_is_data_present(hdr->frame_control)) {
-                        u16 ethertype;
-                        u8 *payload;
-
-                        payload = rx->skb->data +
-                                ieee80211_hdrlen(hdr->frame_control);
-                        ethertype = (payload[6] << 8) | payload[7];
-                        if (cpu_to_be16(ethertype) ==
-                            rx->sdata->control_port_protocol)
+                        unsigned int hdrlen;
+                        __be16 ethertype;
+
+                        hdrlen = ieee80211_hdrlen(hdr->frame_control);
+
+                        if (rx->skb->len < hdrlen + 8)
+                                return RX_DROP_MONITOR;
+
+                        skb_copy_bits(rx->skb, hdrlen + 6, &ethertype, 2);
+                        if (ethertype == rx->sdata->control_port_protocol)
                                 return RX_CONTINUE;
                 }
 
@@ -1459,11 +1466,14 @@ ieee80211_rx_h_defragment(struct ieee80211_rx_data *rx)
 
         hdr = (struct ieee80211_hdr *)rx->skb->data;
         fc = hdr->frame_control;
+
+        if (ieee80211_is_ctl(fc))
+                return RX_CONTINUE;
+
         sc = le16_to_cpu(hdr->seq_ctrl);
         frag = sc & IEEE80211_SCTL_FRAG;
 
         if (likely((!ieee80211_has_morefrags(fc) && frag == 0) ||
-                   (rx->skb)->len < 24 ||
                    is_multicast_ether_addr(hdr->addr1))) {
                 /* not fragmented */
                 goto out;
@@ -1882,6 +1892,20 @@ ieee80211_rx_h_mesh_fwding(struct ieee80211_rx_data *rx)
 
         hdr = (struct ieee80211_hdr *) skb->data;
         hdrlen = ieee80211_hdrlen(hdr->frame_control);
+
+        /* make sure fixed part of mesh header is there, also checks skb len */
+        if (!pskb_may_pull(rx->skb, hdrlen + 6))
+                return RX_DROP_MONITOR;
+
+        mesh_hdr = (struct ieee80211s_hdr *) (skb->data + hdrlen);
+
+        /* make sure full mesh header is there, also checks skb len */
+        if (!pskb_may_pull(rx->skb,
+                           hdrlen + ieee80211_get_mesh_hdrlen(mesh_hdr)))
+                return RX_DROP_MONITOR;
+
+        /* reload pointers */
+        hdr = (struct ieee80211_hdr *) skb->data;
         mesh_hdr = (struct ieee80211s_hdr *) (skb->data + hdrlen);
 
         /* frame is in RMC, don't forward */
@@ -1890,7 +1914,8 @@ ieee80211_rx_h_mesh_fwding(struct ieee80211_rx_data *rx)
             mesh_rmc_check(hdr->addr3, mesh_hdr, rx->sdata))
                 return RX_DROP_MONITOR;
 
-        if (!ieee80211_is_data(hdr->frame_control))
+        if (!ieee80211_is_data(hdr->frame_control) ||
+            !(status->rx_flags & IEEE80211_RX_RA_MATCH))
                 return RX_CONTINUE;
 
         if (!mesh_hdr->ttl)
@@ -1904,9 +1929,12 @@ ieee80211_rx_h_mesh_fwding(struct ieee80211_rx_data *rx)
                 if (is_multicast_ether_addr(hdr->addr1)) {
                         mpp_addr = hdr->addr3;
                         proxied_addr = mesh_hdr->eaddr1;
-                } else {
+                } else if (mesh_hdr->flags & MESH_FLAGS_AE_A5_A6) {
+                        /* has_a4 already checked in ieee80211_rx_mesh_check */
                         mpp_addr = hdr->addr4;
                         proxied_addr = mesh_hdr->eaddr2;
+                } else {
+                        return RX_DROP_MONITOR;
                 }
 
                 rcu_read_lock();
@@ -1934,12 +1962,9 @@ ieee80211_rx_h_mesh_fwding(struct ieee80211_rx_data *rx)
         }
         skb_set_queue_mapping(skb, q);
 
-        if (!(status->rx_flags & IEEE80211_RX_RA_MATCH))
-                goto out;
-
         if (!--mesh_hdr->ttl) {
                 IEEE80211_IFSTA_MESH_CTR_INC(ifmsh, dropped_frames_ttl);
-                return RX_DROP_MONITOR;
+                goto out;
         }
 
         if (!ifmsh->mshcfg.dot11MeshForwarding)
@@ -2346,6 +2371,10 @@ ieee80211_rx_h_action(struct ieee80211_rx_data *rx)
                 }
                 break;
         case WLAN_CATEGORY_SELF_PROTECTED:
+                if (len < (IEEE80211_MIN_ACTION_SIZE +
+                           sizeof(mgmt->u.action.u.self_prot.action_code)))
+                        break;
+
                 switch (mgmt->u.action.u.self_prot.action_code) {
                 case WLAN_SP_MESH_PEERING_OPEN:
                 case WLAN_SP_MESH_PEERING_CLOSE:
@@ -2364,6 +2393,10 @@ ieee80211_rx_h_action(struct ieee80211_rx_data *rx)
                 }
                 break;
         case WLAN_CATEGORY_MESH_ACTION:
+                if (len < (IEEE80211_MIN_ACTION_SIZE +
+                           sizeof(mgmt->u.action.u.mesh_action.action_code)))
+                        break;
+
                 if (!ieee80211_vif_is_mesh(&sdata->vif))
                         break;
                 if (mesh_action_is_path_sel(mgmt) &&
@@ -2905,10 +2938,15 @@ static void __ieee80211_rx_handle_packet(struct ieee80211_hw *hw,
         if (ieee80211_is_data(fc) || ieee80211_is_mgmt(fc))
                 local->dot11ReceivedFragmentCount++;
 
-        if (ieee80211_is_mgmt(fc))
-                err = skb_linearize(skb);
-        else
+        if (ieee80211_is_mgmt(fc)) {
+                /* drop frame if too short for header */
+                if (skb->len < ieee80211_hdrlen(fc))
+                        err = -ENOBUFS;
+                else
+                        err = skb_linearize(skb);
+        } else {
                 err = !pskb_may_pull(skb, ieee80211_hdrlen(fc));
+        }
 
         if (err) {
                 dev_kfree_skb(skb);
diff --git a/net/mac80211/sta_info.c b/net/mac80211/sta_info.c
index daf55e1e0fd..f7bb54f9ab7 100644
--- a/net/mac80211/sta_info.c
+++ b/net/mac80211/sta_info.c
@@ -664,7 +664,7 @@ static bool sta_info_cleanup_expire_buffered_ac(struct ieee80211_local *local,
                  */
                 if (!skb)
                         break;
-                dev_kfree_skb(skb);
+                ieee80211_free_txskb(&local->hw, skb);
         }
 
         /*
@@ -693,7 +693,7 @@ static bool sta_info_cleanup_expire_buffered_ac(struct ieee80211_local *local,
                 local->total_ps_buffered--;
                 ps_dbg(sta->sdata, "Buffered frame expired (STA %pM)\n",
                        sta->sta.addr);
-                dev_kfree_skb(skb);
+                ieee80211_free_txskb(&local->hw, skb);
         }
 
         /*
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index dd0e6f20fc5..6636d396231 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -406,7 +406,7 @@ void ieee80211_add_pending_skb(struct ieee80211_local *local,
         int queue = info->hw_queue;
 
         if (WARN_ON(!info->control.vif)) {
-                kfree_skb(skb);
+                ieee80211_free_txskb(&local->hw, skb);
                 return;
         }
 
@@ -431,7 +431,7 @@ void ieee80211_add_pending_skbs_fn(struct ieee80211_local *local,
                 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
                 if (WARN_ON(!info->control.vif)) {
-                        kfree_skb(skb);
+                        ieee80211_free_txskb(&local->hw, skb);
                         continue;
                 }
 
@@ -643,13 +643,41 @@ u32 ieee802_11_parse_elems_crc(u8 *start, size_t len,
                         break;
                 }
 
-                if (id != WLAN_EID_VENDOR_SPECIFIC &&
-                    id != WLAN_EID_QUIET &&
-                    test_bit(id, seen_elems)) {
-                        elems->parse_error = true;
-                        left -= elen;
-                        pos += elen;
-                        continue;
+                switch (id) {
+                case WLAN_EID_SSID:
+                case WLAN_EID_SUPP_RATES:
+                case WLAN_EID_FH_PARAMS:
+                case WLAN_EID_DS_PARAMS:
+                case WLAN_EID_CF_PARAMS:
+                case WLAN_EID_TIM:
+                case WLAN_EID_IBSS_PARAMS:
+                case WLAN_EID_CHALLENGE:
+                case WLAN_EID_RSN:
+                case WLAN_EID_ERP_INFO:
+                case WLAN_EID_EXT_SUPP_RATES:
+                case WLAN_EID_HT_CAPABILITY:
+                case WLAN_EID_HT_OPERATION:
+                case WLAN_EID_VHT_CAPABILITY:
+                case WLAN_EID_VHT_OPERATION:
+                case WLAN_EID_MESH_ID:
+                case WLAN_EID_MESH_CONFIG:
+                case WLAN_EID_PEER_MGMT:
+                case WLAN_EID_PREQ:
+                case WLAN_EID_PREP:
+                case WLAN_EID_PERR:
+                case WLAN_EID_RANN:
+                case WLAN_EID_CHANNEL_SWITCH:
+                case WLAN_EID_EXT_CHANSWITCH_ANN:
+                case WLAN_EID_COUNTRY:
+                case WLAN_EID_PWR_CONSTRAINT:
+                case WLAN_EID_TIMEOUT_INTERVAL:
+                        if (test_bit(id, seen_elems)) {
+                                elems->parse_error = true;
+                                left -= elen;
+                                pos += elen;
+                                continue;
+                        }
+                        break;
                 }
 
                 if (calc_crc && id < 64 && (filter & (1ULL << id)))
diff --git a/net/mac80211/wpa.c b/net/mac80211/wpa.c
index bdb53aba888..8bd2f5c6a56 100644
--- a/net/mac80211/wpa.c
+++ b/net/mac80211/wpa.c
@@ -106,7 +106,8 @@ ieee80211_rx_h_michael_mic_verify(struct ieee80211_rx_data *rx)
                 if (status->flag & RX_FLAG_MMIC_ERROR)
                         goto mic_fail;
 
-                if (!(status->flag & RX_FLAG_IV_STRIPPED) && rx->key)
+                if (!(status->flag & RX_FLAG_IV_STRIPPED) && rx->key &&
+                    rx->key->conf.cipher == WLAN_CIPHER_SUITE_TKIP)
                         goto update_iv;
 
                 return RX_CONTINUE;
@@ -545,14 +546,19 @@ ieee80211_crypto_ccmp_decrypt(struct ieee80211_rx_data *rx)
 
 static void bip_aad(struct sk_buff *skb, u8 *aad)
 {
+        __le16 mask_fc;
+        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+
         /* BIP AAD: FC(masked) || A1 || A2 || A3 */
 
         /* FC type/subtype */
-        aad[0] = skb->data[0];
         /* Mask FC Retry, PwrMgt, MoreData flags to zero */
-        aad[1] = skb->data[1] & ~(BIT(4) | BIT(5) | BIT(6));
+        mask_fc = hdr->frame_control;
+        mask_fc &= ~cpu_to_le16(IEEE80211_FCTL_RETRY | IEEE80211_FCTL_PM |
+                                IEEE80211_FCTL_MOREDATA);
+        put_unaligned(mask_fc, (__le16 *) &aad[0]);
         /* A1 || A2 || A3 */
-        memcpy(aad + 2, skb->data + 4, 3 * ETH_ALEN);
+        memcpy(aad + 2, &hdr->addr1, 3 * ETH_ALEN);
 }
 
 
diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig
index 8d8d9bc4b6f..60c3bbb63e8 100644
--- a/net/nfc/Kconfig
+++ b/net/nfc/Kconfig
@@ -3,8 +3,8 @@
 #
 
 menuconfig NFC
-        depends on NET && EXPERIMENTAL
-        tristate "NFC subsystem support (EXPERIMENTAL)"
+        depends on NET
+        tristate "NFC subsystem support"
         default n
         help
           Say Y here if you want to build support for NFC (Near field
diff --git a/net/nfc/core.c b/net/nfc/core.c
index 479bee36dc3..aa64ea44167 100644
--- a/net/nfc/core.c
+++ b/net/nfc/core.c
@@ -40,6 +40,9 @@
 int nfc_devlist_generation;
 DEFINE_MUTEX(nfc_devlist_mutex);
 
+/* NFC device ID bitmap */
+static DEFINE_IDA(nfc_index_ida);
+
 /**
  * nfc_dev_up - turn on the NFC device
  *
@@ -181,6 +184,7 @@ int nfc_stop_poll(struct nfc_dev *dev)
 
         dev->ops->stop_poll(dev);
         dev->polling = false;
+        dev->rf_mode = NFC_RF_NONE;
 
 error:
         device_unlock(&dev->dev);
@@ -194,7 +198,7 @@ static struct nfc_target *nfc_find_target(struct nfc_dev *dev, u32 target_idx)
         if (dev->n_targets == 0)
                 return NULL;
 
-        for (i = 0; i < dev->n_targets ; i++) {
+        for (i = 0; i < dev->n_targets; i++) {
                 if (dev->targets[i].idx == target_idx)
                         return &dev->targets[i];
         }
@@ -274,12 +278,14 @@ int nfc_dep_link_down(struct nfc_dev *dev)
         if (!rc) {
                 dev->dep_link_up = false;
                 dev->active_target = NULL;
+                dev->rf_mode = NFC_RF_NONE;
                 nfc_llcp_mac_is_down(dev);
                 nfc_genl_dep_link_down_event(dev);
         }
 
 error:
         device_unlock(&dev->dev);
+
         return rc;
 }
 
@@ -503,6 +509,7 @@ EXPORT_SYMBOL(nfc_tm_activated);
 int nfc_tm_deactivated(struct nfc_dev *dev)
 {
         dev->dep_link_up = false;
+        dev->rf_mode = NFC_RF_NONE;
 
         return nfc_genl_tm_deactivated(dev);
 }
@@ -697,6 +704,8 @@ static void nfc_check_pres_work(struct work_struct *work)
 
         if (dev->active_target && timer_pending(&dev->check_pres_timer) == 0) {
                 rc = dev->ops->check_presence(dev, dev->active_target);
+                if (rc == -EOPNOTSUPP)
+                        goto exit;
                 if (!rc) {
                         mod_timer(&dev->check_pres_timer, jiffies +
                                   msecs_to_jiffies(NFC_CHECK_PRES_FREQ_MS));
@@ -708,6 +717,7 @@ static void nfc_check_pres_work(struct work_struct *work)
                 }
         }
 
+exit:
         device_unlock(&dev->dev);
 }
 
@@ -753,7 +763,6 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
                                     u32 supported_protocols,
                                     int tx_headroom, int tx_tailroom)
 {
-        static atomic_t dev_no = ATOMIC_INIT(0);
         struct nfc_dev *dev;
 
         if (!ops->start_poll || !ops->stop_poll || !ops->activate_target ||
@@ -767,11 +776,6 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
         if (!dev)
                 return NULL;
 
-        dev->dev.class = &nfc_class;
-        dev->idx = atomic_inc_return(&dev_no) - 1;
-        dev_set_name(&dev->dev, "nfc%d", dev->idx);
-        device_initialize(&dev->dev);
-
         dev->ops = ops;
         dev->supported_protocols = supported_protocols;
         dev->tx_headroom = tx_headroom;
@@ -779,6 +783,7 @@ struct nfc_dev *nfc_allocate_device(struct nfc_ops *ops,
 
         nfc_genl_data_init(&dev->genl_data);
 
+        dev->rf_mode = NFC_RF_NONE;
 
         /* first generation must not be 0 */
         dev->targets_generation = 1;
@@ -806,6 +811,14 @@ int nfc_register_device(struct nfc_dev *dev)
 
         pr_debug("dev_name=%s\n", dev_name(&dev->dev));
 
+        dev->idx = ida_simple_get(&nfc_index_ida, 0, 0, GFP_KERNEL);
+        if (dev->idx < 0)
+                return dev->idx;
+
+        dev->dev.class = &nfc_class;
+        dev_set_name(&dev->dev, "nfc%d", dev->idx);
+        device_initialize(&dev->dev);
+
         mutex_lock(&nfc_devlist_mutex);
         nfc_devlist_generation++;
         rc = device_add(&dev->dev);
@@ -834,10 +847,12 @@ EXPORT_SYMBOL(nfc_register_device);
  */
 void nfc_unregister_device(struct nfc_dev *dev)
 {
-        int rc;
+        int rc, id;
 
         pr_debug("dev_name=%s\n", dev_name(&dev->dev));
 
+        id = dev->idx;
+
         mutex_lock(&nfc_devlist_mutex);
         nfc_devlist_generation++;
 
@@ -856,6 +871,8 @@ void nfc_unregister_device(struct nfc_dev *dev)
                 pr_debug("The userspace won't be notified that the device %s was removed\n",
                          dev_name(&dev->dev));
 
+        ida_simple_remove(&nfc_index_ida, id);
+
 }
 EXPORT_SYMBOL(nfc_unregister_device);
 
diff --git a/net/nfc/hci/command.c b/net/nfc/hci/command.c
index 71c6a7086b8..07659cfd6d7 100644
--- a/net/nfc/hci/command.c
+++ b/net/nfc/hci/command.c
@@ -257,16 +257,16 @@ static u8 nfc_hci_create_pipe(struct nfc_hci_dev *hdev, u8 dest_host,
         *result = nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
                                       NFC_HCI_ADM_CREATE_PIPE,
                                       (u8 *) &params, sizeof(params), &skb);
-        if (*result == 0) {
-                resp = (struct hci_create_pipe_resp *)skb->data;
-                pipe = resp->pipe;
-                kfree_skb(skb);
+        if (*result < 0)
+                return NFC_HCI_INVALID_PIPE;
 
-                pr_debug("pipe created=%d\n", pipe);
+        resp = (struct hci_create_pipe_resp *)skb->data;
+        pipe = resp->pipe;
+        kfree_skb(skb);
 
-                return pipe;
-        } else
-                return NFC_HCI_INVALID_PIPE;
+        pr_debug("pipe created=%d\n", pipe);
+
+        return pipe;
 }
 
 static int nfc_hci_delete_pipe(struct nfc_hci_dev *hdev, u8 pipe)
@@ -279,8 +279,6 @@ static int nfc_hci_delete_pipe(struct nfc_hci_dev *hdev, u8 pipe)
 
 static int nfc_hci_clear_all_pipes(struct nfc_hci_dev *hdev)
 {
-        int r;
-
         u8 param[2];
 
         /* TODO: Find out what the identity reference data is
@@ -288,10 +286,8 @@ static int nfc_hci_clear_all_pipes(struct nfc_hci_dev *hdev)
 
         pr_debug("\n");
 
-        r = nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
-                                NFC_HCI_ADM_CLEAR_ALL_PIPE, param, 2, NULL);
-
-        return 0;
+        return nfc_hci_execute_cmd(hdev, NFC_HCI_ADMIN_PIPE,
+                                   NFC_HCI_ADM_CLEAR_ALL_PIPE, param, 2, NULL);
 }
 
 int nfc_hci_disconnect_gate(struct nfc_hci_dev *hdev, u8 gate)
diff --git a/net/nfc/hci/core.c b/net/nfc/hci/core.c
index 5fbb6e40793..bc571b0efb9 100644
--- a/net/nfc/hci/core.c
+++ b/net/nfc/hci/core.c
@@ -65,8 +65,9 @@ static void nfc_hci_msg_tx_work(struct work_struct *work)
                                                           -ETIME);
                         kfree(hdev->cmd_pending_msg);
                         hdev->cmd_pending_msg = NULL;
-                } else
+                } else {
                         goto exit;
+                }
         }
 
 next_msg:
@@ -182,7 +183,7 @@ static u32 nfc_hci_sak_to_protocol(u8 sak)
         }
 }
 
-static int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
+int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
 {
         struct nfc_target *targets;
         struct sk_buff *atqa_skb = NULL;
@@ -263,7 +264,9 @@ static int nfc_hci_target_discovered(struct nfc_hci_dev *hdev, u8 gate)
                 break;
         }
 
-        targets->hci_reader_gate = gate;
+        /* if driver set the new gate, we will skip the old one */
+        if (targets->hci_reader_gate == 0x00)
+                targets->hci_reader_gate = gate;
 
         r = nfc_targets_found(hdev->ndev, targets, 1);
 
@@ -275,6 +278,7 @@ exit:
 
         return r;
 }
+EXPORT_SYMBOL(nfc_hci_target_discovered);
 
 void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event,
                             struct sk_buff *skb)
@@ -307,8 +311,13 @@ void nfc_hci_event_received(struct nfc_hci_dev *hdev, u8 pipe, u8 event,
                                               nfc_hci_pipe2gate(hdev, pipe));
                 break;
         default:
-                /* TODO: Unknown events are hardware specific
-                 * pass them to the driver (needs a new hci_ops) */
+                if (hdev->ops->event_received) {
+                        hdev->ops->event_received(hdev,
+                                                nfc_hci_pipe2gate(hdev, pipe),
+                                                event, skb);
+                        return;
+                }
+
                 break;
         }
 
@@ -527,7 +536,8 @@ static int hci_start_poll(struct nfc_dev *nfc_dev,
                 return hdev->ops->start_poll(hdev, im_protocols, tm_protocols);
         else
                 return nfc_hci_send_event(hdev, NFC_HCI_RF_READER_A_GATE,
-                                       NFC_HCI_EVT_READER_REQUESTED, NULL, 0);
+                                          NFC_HCI_EVT_READER_REQUESTED,
+                                          NULL, 0);
 }
 
 static void hci_stop_poll(struct nfc_dev *nfc_dev)
@@ -538,6 +548,28 @@ static void hci_stop_poll(struct nfc_dev *nfc_dev)
                            NFC_HCI_EVT_END_OPERATION, NULL, 0);
 }
 
+static int hci_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
+                                __u8 comm_mode, __u8 *gb, size_t gb_len)
+{
+        struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+        if (hdev->ops->dep_link_up)
+                return hdev->ops->dep_link_up(hdev, target, comm_mode,
+                                                gb, gb_len);
+
+        return 0;
+}
+
+static int hci_dep_link_down(struct nfc_dev *nfc_dev)
+{
+        struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+        if (hdev->ops->dep_link_down)
+                return hdev->ops->dep_link_down(hdev);
+
+        return 0;
+}
+
 static int hci_activate_target(struct nfc_dev *nfc_dev,
                                struct nfc_target *target, u32 protocol)
 {
@@ -586,8 +618,8 @@ static int hci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
         switch (target->hci_reader_gate) {
         case NFC_HCI_RF_READER_A_GATE:
         case NFC_HCI_RF_READER_B_GATE:
-                if (hdev->ops->data_exchange) {
-                        r = hdev->ops->data_exchange(hdev, target, skb, cb,
+                if (hdev->ops->im_transceive) {
+                        r = hdev->ops->im_transceive(hdev, target, skb, cb,
                                                      cb_context);
                         if (r <= 0)     /* handled */
                                 break;
@@ -604,14 +636,14 @@ static int hci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
                                            skb->len, hci_transceive_cb, hdev);
                 break;
         default:
-                if (hdev->ops->data_exchange) {
-                        r = hdev->ops->data_exchange(hdev, target, skb, cb,
+                if (hdev->ops->im_transceive) {
+                        r = hdev->ops->im_transceive(hdev, target, skb, cb,
                                                      cb_context);
                         if (r == 1)
                                 r = -ENOTSUPP;
-                }
-                else
+                } else {
                         r = -ENOTSUPP;
+                }
                 break;
         }
 
@@ -620,6 +652,16 @@ static int hci_transceive(struct nfc_dev *nfc_dev, struct nfc_target *target,
         return r;
 }
 
+static int hci_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb)
+{
+        struct nfc_hci_dev *hdev = nfc_get_drvdata(nfc_dev);
+
+        if (hdev->ops->tm_send)
+                return hdev->ops->tm_send(hdev, skb);
+        else
+                return -ENOTSUPP;
+}
+
 static int hci_check_presence(struct nfc_dev *nfc_dev,
                               struct nfc_target *target)
 {
@@ -723,9 +765,12 @@ static struct nfc_ops hci_nfc_ops = {
         .dev_down = hci_dev_down,
         .start_poll = hci_start_poll,
         .stop_poll = hci_stop_poll,
+        .dep_link_up = hci_dep_link_up,
+        .dep_link_down = hci_dep_link_down,
         .activate_target = hci_activate_target,
         .deactivate_target = hci_deactivate_target,
         .im_transceive = hci_transceive,
+        .tm_send = hci_tm_send,
         .check_presence = hci_check_presence,
 };
 
@@ -848,7 +893,7 @@ void nfc_hci_driver_failure(struct nfc_hci_dev *hdev, int err)
 }
 EXPORT_SYMBOL(nfc_hci_driver_failure);
 
-void inline nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb)
+void nfc_hci_recv_frame(struct nfc_hci_dev *hdev, struct sk_buff *skb)
 {
         nfc_llc_rcv_from_drv(hdev->llc, skb);
 }
diff --git a/net/nfc/hci/llc.c b/net/nfc/hci/llc.c
index ae1205ded87..fe5e966e5b8 100644
--- a/net/nfc/hci/llc.c
+++ b/net/nfc/hci/llc.c
@@ -72,7 +72,7 @@ int nfc_llc_register(const char *name, struct nfc_llc_ops *ops)
         llc_engine->ops = ops;
 
         INIT_LIST_HEAD(&llc_engine->entry);
-        list_add_tail (&llc_engine->entry, &llc_engines);
+        list_add_tail(&llc_engine->entry, &llc_engines);
 
         return 0;
 }
diff --git a/net/nfc/hci/llc_shdlc.c b/net/nfc/hci/llc_shdlc.c
index 01cbc72943c..27b313befc3 100644
--- a/net/nfc/hci/llc_shdlc.c
+++ b/net/nfc/hci/llc_shdlc.c
@@ -634,9 +634,9 @@ static void llc_shdlc_sm_work(struct work_struct *work)
                         r = llc_shdlc_connect_initiate(shdlc);
                 else
                         r = -ETIME;
-                if (r < 0)
+                if (r < 0) {
                         llc_shdlc_connect_complete(shdlc, r);
-                else {
+                } else {
                         mod_timer(&shdlc->connect_timer, jiffies +
                                   msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
 
@@ -682,9 +682,8 @@ static void llc_shdlc_sm_work(struct work_struct *work)
                         llc_shdlc_handle_send_queue(shdlc);
                 }
 
-                if (shdlc->hard_fault) {
+                if (shdlc->hard_fault)
                         shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);
-                }
                 break;
         default:
                 break;
diff --git a/net/nfc/llcp/Kconfig b/net/nfc/llcp/Kconfig
index fbf5e815090..a1a41cd6825 100644
--- a/net/nfc/llcp/Kconfig
+++ b/net/nfc/llcp/Kconfig
@@ -1,6 +1,6 @@
 config NFC_LLCP
-       depends on NFC && EXPERIMENTAL
-       bool "NFC LLCP support (EXPERIMENTAL)"
+       depends on NFC
+       bool "NFC LLCP support"
        default n
        help
          Say Y here if you want to build support for a kernel NFC LLCP
diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
index c45ccd6c094..ed2d17312d6 100644
--- a/net/nfc/llcp/commands.c
+++ b/net/nfc/llcp/commands.c
@@ -261,7 +261,6 @@ int nfc_llcp_disconnect(struct nfc_llcp_sock *sock)
         struct sk_buff *skb;
         struct nfc_dev *dev;
         struct nfc_llcp_local *local;
-        u16 size = 0;
 
         pr_debug("Sending DISC\n");
 
@@ -273,17 +272,10 @@ int nfc_llcp_disconnect(struct nfc_llcp_sock *sock)
         if (dev == NULL)
                 return -ENODEV;
 
-        size += LLCP_HEADER_SIZE;
-        size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
-
-        skb = alloc_skb(size, GFP_ATOMIC);
+        skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
         if (skb == NULL)
                 return -ENOMEM;
 
-        skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
-
-        skb = llcp_add_header(skb, sock->dsap, sock->ssap, LLCP_PDU_DISC);
-
         skb_queue_tail(&local->tx_queue, skb);
 
         return 0;
@@ -324,8 +316,7 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
         struct sk_buff *skb;
         u8 *service_name_tlv = NULL, service_name_tlv_length;
         u8 *miux_tlv = NULL, miux_tlv_length;
-        u8 *rw_tlv = NULL, rw_tlv_length, rw;
-        __be16 miux;
+        u8 *rw_tlv = NULL, rw_tlv_length;
         int err;
         u16 size = 0;
 
@@ -343,13 +334,11 @@ int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
                 size += service_name_tlv_length;
         }
 
-        miux = cpu_to_be16(LLCP_MAX_MIUX);
-        miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+        miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
                                       &miux_tlv_length);
         size += miux_tlv_length;
 
-        rw = LLCP_MAX_RW;
-        rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
+        rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &local->rw, 0, &rw_tlv_length);
         size += rw_tlv_length;
 
         pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len);
@@ -386,8 +375,7 @@ int nfc_llcp_send_cc(struct nfc_llcp_sock *sock)
         struct nfc_llcp_local *local;
         struct sk_buff *skb;
         u8 *miux_tlv = NULL, miux_tlv_length;
-        u8 *rw_tlv = NULL, rw_tlv_length, rw;
-        __be16 miux;
+        u8 *rw_tlv = NULL, rw_tlv_length;
         int err;
         u16 size = 0;
 
@@ -397,13 +385,11 @@ int nfc_llcp_send_cc(struct nfc_llcp_sock *sock)
         if (local == NULL)
                 return -ENODEV;
 
-        miux = cpu_to_be16(LLCP_MAX_MIUX);
-        miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+        miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
                                       &miux_tlv_length);
         size += miux_tlv_length;
 
-        rw = LLCP_MAX_RW;
-        rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
+        rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &local->rw, 0, &rw_tlv_length);
         size += rw_tlv_length;
 
         skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size);
@@ -428,6 +414,52 @@ error_tlv:
         return err;
 }
 
+int nfc_llcp_send_snl(struct nfc_llcp_local *local, u8 tid, u8 sap)
+{
+        struct sk_buff *skb;
+        struct nfc_dev *dev;
+        u8 *sdres_tlv = NULL, sdres_tlv_length, sdres[2];
+        u16 size = 0;
+
+        pr_debug("Sending SNL tid 0x%x sap 0x%x\n", tid, sap);
+
+        if (local == NULL)
+                return -ENODEV;
+
+        dev = local->dev;
+        if (dev == NULL)
+                return -ENODEV;
+
+        sdres[0] = tid;
+        sdres[1] = sap;
+        sdres_tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, sdres, 0,
+                                       &sdres_tlv_length);
+        if (sdres_tlv == NULL)
+                return -ENOMEM;
+
+        size += LLCP_HEADER_SIZE;
+        size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+        size += sdres_tlv_length;
+
+        skb = alloc_skb(size, GFP_KERNEL);
+        if (skb == NULL) {
+                kfree(sdres_tlv);
+                return -ENOMEM;
+        }
+
+        skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+        skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL);
+
+        memcpy(skb_put(skb, sdres_tlv_length), sdres_tlv, sdres_tlv_length);
+
+        skb_queue_tail(&local->tx_queue, skb);
+
+        kfree(sdres_tlv);
+
+        return 0;
+}
+
 int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
 {
         struct sk_buff *skb;
@@ -541,6 +573,52 @@ int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
         return len;
 }
 
+int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+                           struct msghdr *msg, size_t len)
+{
+        struct sk_buff *pdu;
+        struct nfc_llcp_local *local;
+        size_t frag_len = 0, remaining_len;
+        u8 *msg_ptr;
+        int err;
+
+        pr_debug("Send UI frame len %zd\n", len);
+
+        local = sock->local;
+        if (local == NULL)
+                return -ENODEV;
+
+        remaining_len = len;
+        msg_ptr = (u8 *) msg->msg_iov;
+
+        while (remaining_len > 0) {
+
+                frag_len = min_t(size_t, sock->miu, remaining_len);
+
+                pr_debug("Fragment %zd bytes remaining %zd",
+                         frag_len, remaining_len);
+
+                pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
+                                         frag_len + LLCP_HEADER_SIZE, &err);
+                if (pdu == NULL) {
+                        pr_err("Could not allocate PDU\n");
+                        continue;
+                }
+
+                pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI);
+
+                memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
+
+                /* No need to check for the peer RW for UI frames */
+                skb_queue_tail(&local->tx_queue, pdu);
+
+                remaining_len -= frag_len;
+                msg_ptr += frag_len;
+        }
+
+        return len;
+}
+
 int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
 {
         struct sk_buff *skb;
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index cc10d073c33..f6804532047 100644
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -45,12 +45,38 @@ void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
         write_unlock(&l->lock);
 }
 
+static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock)
+{
+        struct nfc_llcp_local *local = sock->local;
+        struct sk_buff *s, *tmp;
+
+        pr_debug("%p\n", &sock->sk);
+
+        skb_queue_purge(&sock->tx_queue);
+        skb_queue_purge(&sock->tx_pending_queue);
+        skb_queue_purge(&sock->tx_backlog_queue);
+
+        if (local == NULL)
+                return;
+
+        /* Search for local pending SKBs that are related to this socket */
+        skb_queue_walk_safe(&local->tx_queue, s, tmp) {
+                if (s->sk != &sock->sk)
+                        continue;
+
+                skb_unlink(s, &local->tx_queue);
+                kfree_skb(s);
+        }
+}
+
 static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
 {
         struct sock *sk;
         struct hlist_node *node, *tmp;
         struct nfc_llcp_sock *llcp_sock;
 
+        skb_queue_purge(&local->tx_queue);
+
         write_lock(&local->sockets.lock);
 
         sk_for_each_safe(sk, node, tmp, &local->sockets.head) {
@@ -58,6 +84,8 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
 
                 bh_lock_sock(sk);
 
+                nfc_llcp_socket_purge(llcp_sock);
+
                 if (sk->sk_state == LLCP_CONNECTED)
                         nfc_put_device(llcp_sock->dev);
 
@@ -65,7 +93,8 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
                         struct nfc_llcp_sock *lsk, *n;
                         struct sock *accept_sk;
 
-                        list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
+                        list_for_each_entry_safe(lsk, n,
+                                                 &llcp_sock->accept_queue,
                                                  accept_queue) {
                                 accept_sk = &lsk->sk;
                                 bh_lock_sock(accept_sk);
@@ -85,6 +114,16 @@ static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool listen)
                         }
                 }
 
+                /*
+                 * If we have a connection less socket bound, we keep it alive
+                 * if the device is still present.
+                 */
+                if (sk->sk_state == LLCP_BOUND && sk->sk_type == SOCK_DGRAM &&
+                    listen == true) {
+                        bh_unlock_sock(sk);
+                        continue;
+                }
+
                 sk->sk_state = LLCP_CLOSED;
 
                 bh_unlock_sock(sk);
@@ -134,7 +173,7 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
 {
         struct sock *sk;
         struct hlist_node *node;
-        struct nfc_llcp_sock *llcp_sock;
+        struct nfc_llcp_sock *llcp_sock, *tmp_sock;
 
         pr_debug("ssap dsap %d %d\n", ssap, dsap);
 
@@ -146,10 +185,12 @@ static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
         llcp_sock = NULL;
 
         sk_for_each(sk, node, &local->sockets.head) {
-                llcp_sock = nfc_llcp_sock(sk);
+                tmp_sock = nfc_llcp_sock(sk);
 
-                if (llcp_sock->ssap == ssap && llcp_sock->dsap == dsap)
+                if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) {
+                        llcp_sock = tmp_sock;
                         break;
+                }
         }
 
         read_unlock(&local->sockets.lock);
@@ -249,7 +290,12 @@ struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
 
                 pr_debug("llcp sock %p\n", tmp_sock);
 
-                if (tmp_sock->sk.sk_state != LLCP_LISTEN)
+                if (tmp_sock->sk.sk_type == SOCK_STREAM &&
+                    tmp_sock->sk.sk_state != LLCP_LISTEN)
+                        continue;
+
+                if (tmp_sock->sk.sk_type == SOCK_DGRAM &&
+                    tmp_sock->sk.sk_state != LLCP_BOUND)
                         continue;
 
                 if (tmp_sock->service_name == NULL ||
@@ -421,10 +467,9 @@ static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
 static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
 {
         u8 *gb_cur, *version_tlv, version, version_length;
-        u8 *lto_tlv, lto, lto_length;
+        u8 *lto_tlv, lto_length;
         u8 *wks_tlv, wks_length;
         u8 *miux_tlv, miux_length;
-        __be16 miux;
         u8 gb_len = 0;
         int ret = 0;
 
@@ -433,9 +478,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
                                          1, &version_length);
         gb_len += version_length;
 
-        /* 1500 ms */
-        lto = 150;
-        lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &lto, 1, &lto_length);
+        lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->lto, 1, &lto_length);
         gb_len += lto_length;
 
         pr_debug("Local wks 0x%lx\n", local->local_wks);
@@ -443,8 +486,7 @@ static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
                                      &wks_length);
         gb_len += wks_length;
 
-        miux = cpu_to_be16(LLCP_MAX_MIUX);
-        miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+        miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
                                       &miux_length);
         gb_len += miux_length;
 
@@ -610,7 +652,10 @@ static void nfc_llcp_tx_work(struct work_struct *work)
         if (skb != NULL) {
                 sk = skb->sk;
                 llcp_sock = nfc_llcp_sock(sk);
-                if (llcp_sock != NULL) {
+
+                if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
+                        nfc_llcp_send_symm(local->dev);
+                } else {
                         int ret;
 
                         pr_debug("Sending pending skb\n");
@@ -629,8 +674,6 @@ static void nfc_llcp_tx_work(struct work_struct *work)
                                 skb_queue_tail(&llcp_sock->tx_pending_queue,
                                                skb);
                         }
-                } else {
-                        nfc_llcp_send_symm(local->dev);
                 }
         } else {
                 nfc_llcp_send_symm(local->dev);
@@ -704,6 +747,39 @@ static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
         return NULL;
 }
 
+static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
+                             struct sk_buff *skb)
+{
+        struct nfc_llcp_sock *llcp_sock;
+        struct nfc_llcp_ui_cb *ui_cb;
+        u8 dsap, ssap;
+
+        dsap = nfc_llcp_dsap(skb);
+        ssap = nfc_llcp_ssap(skb);
+
+        ui_cb = nfc_llcp_ui_skb_cb(skb);
+        ui_cb->dsap = dsap;
+        ui_cb->ssap = ssap;
+
+        printk("%s %d %d\n", __func__, dsap, ssap);
+
+        pr_debug("%d %d\n", dsap, ssap);
+
+        /* We're looking for a bound socket, not a client one */
+        llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+        if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM)
+                return;
+
+        /* There is no sequence with UI frames */
+        skb_pull(skb, LLCP_HEADER_SIZE);
+        if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
+                pr_err("receive queue is full\n");
+                skb_queue_head(&llcp_sock->tx_backlog_queue, skb);
+        }
+
+        nfc_llcp_sock_put(llcp_sock);
+}
+
 static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
                                   struct sk_buff *skb)
 {
@@ -823,9 +899,6 @@ static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
 fail:
         /* Send DM */
         nfc_llcp_send_dm(local, dsap, ssap, reason);
-
-        return;
-
 }
 
 int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
@@ -953,6 +1026,9 @@ static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
 
         sk = &llcp_sock->sk;
         lock_sock(sk);
+
+        nfc_llcp_socket_purge(llcp_sock);
+
         if (sk->sk_state == LLCP_CLOSED) {
                 release_sock(sk);
                 nfc_llcp_sock_put(llcp_sock);
@@ -1027,7 +1103,7 @@ static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
         }
 
         if (llcp_sock == NULL) {
-                pr_err("Invalid DM\n");
+                pr_debug("Already closed\n");
                 return;
         }
 
@@ -1038,8 +1114,100 @@ static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
         sk->sk_state_change(sk);
 
         nfc_llcp_sock_put(llcp_sock);
+}
 
-        return;
+static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
+                              struct sk_buff *skb)
+{
+        struct nfc_llcp_sock *llcp_sock;
+        u8 dsap, ssap, *tlv, type, length, tid, sap;
+        u16 tlv_len, offset;
+        char *service_name;
+        size_t service_name_len;
+
+        dsap = nfc_llcp_dsap(skb);
+        ssap = nfc_llcp_ssap(skb);
+
+        pr_debug("%d %d\n", dsap, ssap);
+
+        if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) {
+                pr_err("Wrong SNL SAP\n");
+                return;
+        }
+
+        tlv = &skb->data[LLCP_HEADER_SIZE];
+        tlv_len = skb->len - LLCP_HEADER_SIZE;
+        offset = 0;
+
+        while (offset < tlv_len) {
+                type = tlv[0];
+                length = tlv[1];
+
+                switch (type) {
+                case LLCP_TLV_SDREQ:
+                        tid = tlv[2];
+                        service_name = (char *) &tlv[3];
+                        service_name_len = length - 1;
+
+                        pr_debug("Looking for %.16s\n", service_name);
+
+                        if (service_name_len == strlen("urn:nfc:sn:sdp") &&
+                            !strncmp(service_name, "urn:nfc:sn:sdp",
+                                     service_name_len)) {
+                                sap = 1;
+                                goto send_snl;
+                        }
+
+                        llcp_sock = nfc_llcp_sock_from_sn(local, service_name,
+                                                          service_name_len);
+                        if (!llcp_sock) {
+                                sap = 0;
+                                goto send_snl;
+                        }
+
+                        /*
+                         * We found a socket but its ssap has not been reserved
+                         * yet. We need to assign it for good and send a reply.
+                         * The ssap will be freed when the socket is closed.
+                         */
+                        if (llcp_sock->ssap == LLCP_SDP_UNBOUND) {
+                                atomic_t *client_count;
+
+                                sap = nfc_llcp_reserve_sdp_ssap(local);
+
+                                pr_debug("Reserving %d\n", sap);
+
+                                if (sap == LLCP_SAP_MAX) {
+                                        sap = 0;
+                                        goto send_snl;
+                                }
+
+                                client_count =
+                                        &local->local_sdp_cnt[sap -
+                                                              LLCP_WKS_NUM_SAP];
+
+                                atomic_inc(client_count);
+
+                                llcp_sock->ssap = sap;
+                                llcp_sock->reserved_ssap = sap;
+                        } else {
+                                sap = llcp_sock->ssap;
+                        }
+
+                        pr_debug("%p %d\n", llcp_sock, sap);
+
+send_snl:
+                        nfc_llcp_send_snl(local, tid, sap);
+                        break;
+
+                default:
+                        pr_err("Invalid SNL tlv value 0x%x\n", type);
+                        break;
+                }
+
+                offset += length + 2;
+                tlv += length + 2;
+        }
 }
 
 static void nfc_llcp_rx_work(struct work_struct *work)
@@ -1072,6 +1240,11 @@ static void nfc_llcp_rx_work(struct work_struct *work)
                 pr_debug("SYMM\n");
                 break;
 
+        case LLCP_PDU_UI:
+                pr_debug("UI\n");
+                nfc_llcp_recv_ui(local, skb);
+                break;
+
         case LLCP_PDU_CONNECT:
                 pr_debug("CONNECT\n");
                 nfc_llcp_recv_connect(local, skb);
@@ -1092,6 +1265,11 @@ static void nfc_llcp_rx_work(struct work_struct *work)
                 nfc_llcp_recv_dm(local, skb);
                 break;
 
+        case LLCP_PDU_SNL:
+                pr_debug("SNL\n");
+                nfc_llcp_recv_snl(local, skb);
+                break;
+
         case LLCP_PDU_I:
         case LLCP_PDU_RR:
         case LLCP_PDU_RNR:
@@ -1104,8 +1282,6 @@ static void nfc_llcp_rx_work(struct work_struct *work)
         schedule_work(&local->tx_work);
         kfree_skb(local->rx_pending);
         local->rx_pending = NULL;
-
-        return;
 }
 
 void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
@@ -1121,8 +1297,6 @@ void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
         local->rx_pending = skb_get(skb);
         del_timer(&local->link_timer);
         schedule_work(&local->rx_work);
-
-        return;
 }
 
 int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
@@ -1205,6 +1379,10 @@ int nfc_llcp_register_device(struct nfc_dev *ndev)
         rwlock_init(&local->connecting_sockets.lock);
         rwlock_init(&local->raw_sockets.lock);
 
+        local->lto = 150; /* 1500 ms */
+        local->rw = LLCP_MAX_RW;
+        local->miux = cpu_to_be16(LLCP_MAX_MIUX);
+
         nfc_llcp_build_gb(local);
 
         local->remote_miu = LLCP_DEFAULT_MIU;
diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
index fdb2d24e60b..0d62366f8cc 100644
--- a/net/nfc/llcp/llcp.h
+++ b/net/nfc/llcp/llcp.h
@@ -64,6 +64,9 @@ struct nfc_llcp_local {
         u32 target_idx;
         u8 rf_mode;
         u8 comm_mode;
+        u8 lto;
+        u8 rw;
+        __be16 miux;
         unsigned long local_wks;      /* Well known services */
         unsigned long local_sdp;      /* Local services  */
         unsigned long local_sap; /* Local SAPs, not available for discovery */
@@ -124,6 +127,13 @@ struct nfc_llcp_sock {
         struct sock *parent;
 };
 
+struct nfc_llcp_ui_cb {
+        __u8 dsap;
+        __u8 ssap;
+};
+
+#define nfc_llcp_ui_skb_cb(__skb) ((struct nfc_llcp_ui_cb *)&((__skb)->cb[0]))
+
 #define nfc_llcp_sock(sk) ((struct nfc_llcp_sock *) (sk))
 #define nfc_llcp_dev(sk)  (nfc_llcp_sock((sk))->dev)
 
@@ -209,10 +219,13 @@ int nfc_llcp_disconnect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_symm(struct nfc_dev *dev);
 int nfc_llcp_send_connect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_cc(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_snl(struct nfc_llcp_local *local, u8 tid, u8 sap);
 int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason);
 int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock);
 int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
                           struct msghdr *msg, size_t len);
+int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+                           struct msghdr *msg, size_t len);
 int nfc_llcp_send_rr(struct nfc_llcp_sock *sock);
 
 /* Socket API */
diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
index 63e4cdc9237..0fa1e92ceac 100644
--- a/net/nfc/llcp/sock.c
+++ b/net/nfc/llcp/sock.c
@@ -205,8 +205,8 @@ static int llcp_sock_listen(struct socket *sock, int backlog)
 
         lock_sock(sk);
 
-        if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM)
-            || sk->sk_state != LLCP_BOUND) {
+        if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) ||
+            sk->sk_state != LLCP_BOUND) {
                 ret = -EBADFD;
                 goto error;
         }
@@ -608,6 +608,25 @@ static int llcp_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
 
         lock_sock(sk);
 
+        if (sk->sk_type == SOCK_DGRAM) {
+                struct sockaddr_nfc_llcp *addr =
+                        (struct sockaddr_nfc_llcp *)msg->msg_name;
+
+                if (msg->msg_namelen < sizeof(*addr)) {
+                        release_sock(sk);
+
+                        pr_err("Invalid socket address length %d\n",
+                               msg->msg_namelen);
+
+                        return -EINVAL;
+                }
+
+                release_sock(sk);
+
+                return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap,
+                                              msg, len);
+        }
+
         if (sk->sk_state != LLCP_CONNECTED) {
                 release_sock(sk);
                 return -ENOTCONN;
@@ -663,11 +682,28 @@ static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
                 return -EFAULT;
         }
 
+        if (sk->sk_type == SOCK_DGRAM && msg->msg_name) {
+                struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb);
+                struct sockaddr_nfc_llcp sockaddr;
+
+                pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap);
+
+                sockaddr.sa_family = AF_NFC;
+                sockaddr.nfc_protocol = NFC_PROTO_NFC_DEP;
+                sockaddr.dsap = ui_cb->dsap;
+                sockaddr.ssap = ui_cb->ssap;
+
+                memcpy(msg->msg_name, &sockaddr, sizeof(sockaddr));
+                msg->msg_namelen = sizeof(sockaddr);
+        }
+
         /* Mark read part of skb as used */
         if (!(flags & MSG_PEEK)) {
 
                 /* SOCK_STREAM: re-queue skb if it contains unreceived data */
-                if (sk->sk_type == SOCK_STREAM || sk->sk_type == SOCK_RAW) {
+                if (sk->sk_type == SOCK_STREAM ||
+                    sk->sk_type == SOCK_DGRAM ||
+                    sk->sk_type == SOCK_RAW) {
                         skb_pull(skb, copied);
                         if (skb->len) {
                                 skb_queue_head(&sk->sk_receive_queue, skb);
diff --git a/net/nfc/nci/Kconfig b/net/nfc/nci/Kconfig
index decdc49b26d..6d69b5f0f19 100644
--- a/net/nfc/nci/Kconfig
+++ b/net/nfc/nci/Kconfig
@@ -1,6 +1,6 @@
 config NFC_NCI
-        depends on NFC && EXPERIMENTAL
-        tristate "NCI protocol support (EXPERIMENTAL)"
+        depends on NFC
+        tristate "NCI protocol support"
         default n
         help
           NCI (NFC Controller Interface) is a communication protocol between
diff --git a/net/nfc/nci/core.c b/net/nfc/nci/core.c
index acf9abb7d99..5f98dc1bf03 100644
--- a/net/nfc/nci/core.c
+++ b/net/nfc/nci/core.c
@@ -205,10 +205,10 @@ static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt)
         cmd.num_disc_configs = 0;
 
         if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
-            (protocols & NFC_PROTO_JEWEL_MASK
-             || protocols & NFC_PROTO_MIFARE_MASK
-             || protocols & NFC_PROTO_ISO14443_MASK
-             || protocols & NFC_PROTO_NFC_DEP_MASK)) {
+            (protocols & NFC_PROTO_JEWEL_MASK ||
+             protocols & NFC_PROTO_MIFARE_MASK ||
+             protocols & NFC_PROTO_ISO14443_MASK ||
+             protocols & NFC_PROTO_NFC_DEP_MASK)) {
                 cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
                         NCI_NFC_A_PASSIVE_POLL_MODE;
                 cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
@@ -224,8 +224,8 @@ static void nci_rf_discover_req(struct nci_dev *ndev, unsigned long opt)
         }
 
         if ((cmd.num_disc_configs < NCI_MAX_NUM_RF_CONFIGS) &&
-            (protocols & NFC_PROTO_FELICA_MASK
-             || protocols & NFC_PROTO_NFC_DEP_MASK)) {
+            (protocols & NFC_PROTO_FELICA_MASK ||
+             protocols & NFC_PROTO_NFC_DEP_MASK)) {
                 cmd.disc_configs[cmd.num_disc_configs].rf_tech_and_mode =
                         NCI_NFC_F_PASSIVE_POLL_MODE;
                 cmd.disc_configs[cmd.num_disc_configs].frequency = 1;
@@ -414,13 +414,13 @@ static int nci_set_local_general_bytes(struct nfc_dev *nfc_dev)
         struct nci_dev *ndev = nfc_get_drvdata(nfc_dev);
         struct nci_set_config_param param;
         __u8 local_gb[NFC_MAX_GT_LEN];
-        int i, rc = 0;
+        int i;
 
         param.val = nfc_get_local_general_bytes(nfc_dev, &param.len);
         if ((param.val == NULL) || (param.len == 0))
-                return rc;
+                return 0;
 
-        if (param.len > NCI_MAX_PARAM_LEN)
+        if (param.len > NFC_MAX_GT_LEN)
                 return -EINVAL;
 
         for (i = 0; i < param.len; i++)
@@ -429,10 +429,8 @@ static int nci_set_local_general_bytes(struct nfc_dev *nfc_dev)
         param.id = NCI_PN_ATR_REQ_GEN_BYTES;
         param.val = local_gb;
 
-        rc = nci_request(ndev, nci_set_config_req, (unsigned long)&param,
-                         msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
-
-        return rc;
+        return nci_request(ndev, nci_set_config_req, (unsigned long)&param,
+                           msecs_to_jiffies(NCI_SET_CONFIG_TIMEOUT));
 }
 
 static int nci_start_poll(struct nfc_dev *nfc_dev,
@@ -579,7 +577,6 @@ static void nci_deactivate_target(struct nfc_dev *nfc_dev,
         }
 }
 
-
 static int nci_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
                            __u8 comm_mode, __u8 *gb, size_t gb_len)
 {
@@ -806,8 +803,8 @@ int nci_recv_frame(struct sk_buff *skb)
 
         pr_debug("len %d\n", skb->len);
 
-        if (!ndev || (!test_bit(NCI_UP, &ndev->flags)
-                      && !test_bit(NCI_INIT, &ndev->flags))) {
+        if (!ndev || (!test_bit(NCI_UP, &ndev->flags) &&
+            !test_bit(NCI_INIT, &ndev->flags))) {
                 kfree_skb(skb);
                 return -ENXIO;
         }
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index c1b5285cbde..3568ae16786 100644
--- a/net/nfc/netlink.c
+++ b/net/nfc/netlink.c
@@ -29,6 +29,8 @@
 
 #include "nfc.h"
 
+#include "llcp/llcp.h"
+
 static struct genl_multicast_group nfc_genl_event_mcgrp = {
         .name = NFC_GENL_MCAST_EVENT_NAME,
 };
@@ -364,7 +366,8 @@ static int nfc_genl_send_device(struct sk_buff *msg, struct nfc_dev *dev,
         if (nla_put_string(msg, NFC_ATTR_DEVICE_NAME, nfc_device_name(dev)) ||
             nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx) ||
             nla_put_u32(msg, NFC_ATTR_PROTOCOLS, dev->supported_protocols) ||
-            nla_put_u8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up))
+            nla_put_u8(msg, NFC_ATTR_DEVICE_POWERED, dev->dev_up) ||
+            nla_put_u8(msg, NFC_ATTR_RF_MODE, dev->rf_mode))
                 goto nla_put_failure;
 
         return genlmsg_end(msg, hdr);
@@ -590,7 +593,7 @@ static int nfc_genl_start_poll(struct sk_buff *skb, struct genl_info *info)
         if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
             ((!info->attrs[NFC_ATTR_IM_PROTOCOLS] &&
               !info->attrs[NFC_ATTR_PROTOCOLS]) &&
-             !info->attrs[NFC_ATTR_TM_PROTOCOLS]))
+              !info->attrs[NFC_ATTR_TM_PROTOCOLS]))
                 return -EINVAL;
 
         idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
@@ -715,6 +718,146 @@ static int nfc_genl_dep_link_down(struct sk_buff *skb, struct genl_info *info)
         return rc;
 }
 
+static int nfc_genl_send_params(struct sk_buff *msg,
+                                struct nfc_llcp_local *local,
+                                u32 portid, u32 seq)
+{
+        void *hdr;
+
+        hdr = genlmsg_put(msg, portid, seq, &nfc_genl_family, 0,
+                          NFC_CMD_LLC_GET_PARAMS);
+        if (!hdr)
+                return -EMSGSIZE;
+
+        if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, local->dev->idx) ||
+            nla_put_u8(msg, NFC_ATTR_LLC_PARAM_LTO, local->lto) ||
+            nla_put_u8(msg, NFC_ATTR_LLC_PARAM_RW, local->rw) ||
+            nla_put_u16(msg, NFC_ATTR_LLC_PARAM_MIUX, be16_to_cpu(local->miux)))
+                goto nla_put_failure;
+
+        return genlmsg_end(msg, hdr);
+
+nla_put_failure:
+
+        genlmsg_cancel(msg, hdr);
+        return -EMSGSIZE;
+}
+
+static int nfc_genl_llc_get_params(struct sk_buff *skb, struct genl_info *info)
+{
+        struct nfc_dev *dev;
+        struct nfc_llcp_local *local;
+        int rc = 0;
+        struct sk_buff *msg = NULL;
+        u32 idx;
+
+        if (!info->attrs[NFC_ATTR_DEVICE_INDEX])
+                return -EINVAL;
+
+        idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+        dev = nfc_get_device(idx);
+        if (!dev)
+                return -ENODEV;
+
+        device_lock(&dev->dev);
+
+        local = nfc_llcp_find_local(dev);
+        if (!local) {
+                rc = -ENODEV;
+                goto exit;
+        }
+
+        msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+        if (!msg) {
+                rc = -ENOMEM;
+                goto exit;
+        }
+
+        rc = nfc_genl_send_params(msg, local, info->snd_portid, info->snd_seq);
+
+exit:
+        device_unlock(&dev->dev);
+
+        nfc_put_device(dev);
+
+        if (rc < 0) {
+                if (msg)
+                        nlmsg_free(msg);
+
+                return rc;
+        }
+
+        return genlmsg_reply(msg, info);
+}
+
+static int nfc_genl_llc_set_params(struct sk_buff *skb, struct genl_info *info)
+{
+        struct nfc_dev *dev;
+        struct nfc_llcp_local *local;
+        u8 rw = 0;
+        u16 miux = 0;
+        u32 idx;
+        int rc = 0;
+
+        if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+            (!info->attrs[NFC_ATTR_LLC_PARAM_LTO] &&
+             !info->attrs[NFC_ATTR_LLC_PARAM_RW] &&
+             !info->attrs[NFC_ATTR_LLC_PARAM_MIUX]))
+                return -EINVAL;
+
+        if (info->attrs[NFC_ATTR_LLC_PARAM_RW]) {
+                rw = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_RW]);
+
+                if (rw > LLCP_MAX_RW)
+                        return -EINVAL;
+        }
+
+        if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX]) {
+                miux = nla_get_u16(info->attrs[NFC_ATTR_LLC_PARAM_MIUX]);
+
+                if (miux > LLCP_MAX_MIUX)
+                        return -EINVAL;
+        }
+
+        idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+
+        dev = nfc_get_device(idx);
+        if (!dev)
+                return -ENODEV;
+
+        device_lock(&dev->dev);
+
+        local = nfc_llcp_find_local(dev);
+        if (!local) {
+                nfc_put_device(dev);
+                rc = -ENODEV;
+                goto exit;
+        }
+
+        if (info->attrs[NFC_ATTR_LLC_PARAM_LTO]) {
+                if (dev->dep_link_up) {
+                        rc = -EINPROGRESS;
+                        goto exit;
+                }
+
+                local->lto = nla_get_u8(info->attrs[NFC_ATTR_LLC_PARAM_LTO]);
+        }
+
+        if (info->attrs[NFC_ATTR_LLC_PARAM_RW])
+                local->rw = rw;
+
+        if (info->attrs[NFC_ATTR_LLC_PARAM_MIUX])
+                local->miux = cpu_to_be16(miux);
+
+exit:
+        device_unlock(&dev->dev);
+
+        nfc_put_device(dev);
+
+        return rc;
+}
+
 static struct genl_ops nfc_genl_ops[] = {
         {
                 .cmd = NFC_CMD_GET_DEVICE,
@@ -759,6 +902,16 @@ static struct genl_ops nfc_genl_ops[] = {
                 .done = nfc_genl_dump_targets_done,
                 .policy = nfc_genl_policy,
         },
+        {
+                .cmd = NFC_CMD_LLC_GET_PARAMS,
+                .doit = nfc_genl_llc_get_params,
+                .policy = nfc_genl_policy,
+        },
+        {
+                .cmd = NFC_CMD_LLC_SET_PARAMS,
+                .doit = nfc_genl_llc_set_params,
+                .policy = nfc_genl_policy,
+        },
 };
 
 
diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h
index c5e42b79a41..87d914d2876 100644
--- a/net/nfc/nfc.h
+++ b/net/nfc/nfc.h
@@ -56,6 +56,7 @@ void nfc_llcp_unregister_device(struct nfc_dev *dev);
 int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len);
 u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len);
 int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb);
+struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
 int __init nfc_llcp_init(void);
 void nfc_llcp_exit(void);
 
@@ -97,6 +98,11 @@ static inline int nfc_llcp_data_received(struct nfc_dev *dev,
         return 0;
 }
 
+static inline struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
+{
+        return NULL;
+}
+
 static inline int nfc_llcp_init(void)
 {
         return 0;
diff --git a/net/nfc/rawsock.c b/net/nfc/rawsock.c
index 8b8a6a2b2ba..313bf1bc848 100644
--- a/net/nfc/rawsock.c
+++ b/net/nfc/rawsock.c
@@ -256,7 +256,6 @@ static int rawsock_recvmsg(struct kiocb *iocb, struct socket *sock,
         return rc ? : copied;
 }
 
-
 static const struct proto_ops rawsock_ops = {
         .family         = PF_NFC,
         .owner          = THIS_MODULE,
diff --git a/net/wireless/core.c b/net/wireless/core.c
index ce1ad776dfb..26711f46a3b 100644
--- a/net/wireless/core.c
+++ b/net/wireless/core.c
@@ -529,8 +529,7 @@ int wiphy_register(struct wiphy *wiphy)
                 for (i = 0; i < sband->n_channels; i++) {
                         sband->channels[i].orig_flags =
                                 sband->channels[i].flags;
-                        sband->channels[i].orig_mag =
-                                sband->channels[i].max_antenna_gain;
+                        sband->channels[i].orig_mag = INT_MAX;
                         sband->channels[i].orig_mpwr =
                                 sband->channels[i].max_power;
                         sband->channels[i].band = band;
diff --git a/net/wireless/mlme.c b/net/wireless/mlme.c
index 46aeafce08d..4bfd14f7c59 100644
--- a/net/wireless/mlme.c
+++ b/net/wireless/mlme.c
@@ -473,20 +473,14 @@ int __cfg80211_mlme_deauth(struct cfg80211_registered_device *rdev,
                 .reason_code = reason,
                 .ie = ie,
                 .ie_len = ie_len,
+                .local_state_change = local_state_change,
         };
 
         ASSERT_WDEV_LOCK(wdev);
 
-        if (local_state_change) {
-                if (wdev->current_bss &&
-                    ether_addr_equal(wdev->current_bss->pub.bssid, bssid)) {
-                        cfg80211_unhold_bss(wdev->current_bss);
-                        cfg80211_put_bss(&wdev->current_bss->pub);
-                        wdev->current_bss = NULL;
-                }
-
+        if (local_state_change && (!wdev->current_bss ||
+            !ether_addr_equal(wdev->current_bss->pub.bssid, bssid)))
                 return 0;
-        }
 
         return rdev_deauth(rdev, dev, &req);
 }
diff --git a/net/wireless/reg.c b/net/wireless/reg.c
index 3b8cbbc214d..bcc7d7ee5a5 100644
--- a/net/wireless/reg.c
+++ b/net/wireless/reg.c
@@ -908,7 +908,7 @@ static void handle_channel(struct wiphy *wiphy,
                         map_regdom_flags(reg_rule->flags) | bw_flags;
                 chan->max_antenna_gain = chan->orig_mag =
                         (int) MBI_TO_DBI(power_rule->max_antenna_gain);
-                chan->max_power = chan->orig_mpwr =
+                chan->max_reg_power = chan->max_power = chan->orig_mpwr =
                         (int) MBM_TO_DBM(power_rule->max_eirp);
                 return;
         }
@@ -1331,7 +1331,8 @@ static void handle_channel_custom(struct wiphy *wiphy,
 
         chan->flags |= map_regdom_flags(reg_rule->flags) | bw_flags;
         chan->max_antenna_gain = (int) MBI_TO_DBI(power_rule->max_antenna_gain);
-        chan->max_power = (int) MBM_TO_DBM(power_rule->max_eirp);
+        chan->max_reg_power = chan->max_power =
+                (int) MBM_TO_DBM(power_rule->max_eirp);
 }
 
 static void handle_band_custom(struct wiphy *wiphy, enum ieee80211_band band,
diff --git a/net/wireless/util.c b/net/wireless/util.c
index 343f13c1d31..5b6c1df72f3 100644
--- a/net/wireless/util.c
+++ b/net/wireless/util.c
@@ -311,23 +311,21 @@ unsigned int ieee80211_get_hdrlen_from_skb(const struct sk_buff *skb)
 }
 EXPORT_SYMBOL(ieee80211_get_hdrlen_from_skb);
 
-static int ieee80211_get_mesh_hdrlen(struct ieee80211s_hdr *meshhdr)
+unsigned int ieee80211_get_mesh_hdrlen(struct ieee80211s_hdr *meshhdr)
 {
         int ae = meshhdr->flags & MESH_FLAGS_AE;
-        /* 7.1.3.5a.2 */
+        /* 802.11-2012, 8.2.4.7.3 */
         switch (ae) {
+        default:
         case 0:
                 return 6;
         case MESH_FLAGS_AE_A4:
                 return 12;
         case MESH_FLAGS_AE_A5_A6:
                 return 18;
-        case (MESH_FLAGS_AE_A4 | MESH_FLAGS_AE_A5_A6):
-                return 24;
-        default:
-                return 6;
         }
 }
+EXPORT_SYMBOL(ieee80211_get_mesh_hdrlen);
 
 int ieee80211_data_to_8023(struct sk_buff *skb, const u8 *addr,
                            enum nl80211_iftype iftype)
@@ -375,6 +373,8 @@ int ieee80211_data_to_8023(struct sk_buff *skb, const u8 *addr,
                         /* make sure meshdr->flags is on the linear part */
                         if (!pskb_may_pull(skb, hdrlen + 1))
                                 return -1;
+                        if (meshdr->flags & MESH_FLAGS_AE_A4)
+                                return -1;
                         if (meshdr->flags & MESH_FLAGS_AE_A5_A6) {
                                 skb_copy_bits(skb, hdrlen +
                                         offsetof(struct ieee80211s_hdr, eaddr1),
@@ -399,6 +399,8 @@ int ieee80211_data_to_8023(struct sk_buff *skb, const u8 *addr,
                         /* make sure meshdr->flags is on the linear part */
                         if (!pskb_may_pull(skb, hdrlen + 1))
                                 return -1;
+                        if (meshdr->flags & MESH_FLAGS_AE_A5_A6)
+                                return -1;
                         if (meshdr->flags & MESH_FLAGS_AE_A4)
                                 skb_copy_bits(skb, hdrlen +
                                         offsetof(struct ieee80211s_hdr, eaddr1),