186 files changed, 15085 insertions, 4189 deletions

diff --git a/drivers/bluetooth/ath3k.c b/drivers/bluetooth/ath3k.c
index 0a327f4154a2..6bfc1bb318f6 100644
--- a/drivers/bluetooth/ath3k.c
+++ b/drivers/bluetooth/ath3k.c
@@ -57,7 +57,7 @@ struct ath3k_version {
         unsigned char   reserved[0x07];
 };
 
-static struct usb_device_id ath3k_table[] = {
+static const struct usb_device_id ath3k_table[] = {
         /* Atheros AR3011 */
         { USB_DEVICE(0x0CF3, 0x3000) },
 
@@ -112,7 +112,7 @@ MODULE_DEVICE_TABLE(usb, ath3k_table);
 #define BTUSB_ATH3012           0x80
 /* This table is to load patch and sysconfig files
  * for AR3012 */
-static struct usb_device_id ath3k_blist_tbl[] = {
+static const struct usb_device_id ath3k_blist_tbl[] = {
 
         /* Atheros AR3012 with sflash firmware*/
         { USB_DEVICE(0x0CF3, 0x0036), .driver_info = BTUSB_ATH3012 },
diff --git a/drivers/bluetooth/bfusb.c b/drivers/bluetooth/bfusb.c
index 995aee9cba22..31386998c9a7 100644
--- a/drivers/bluetooth/bfusb.c
+++ b/drivers/bluetooth/bfusb.c
@@ -42,7 +42,7 @@
 
 static struct usb_driver bfusb_driver;
 
-static struct usb_device_id bfusb_table[] = {
+static const struct usb_device_id bfusb_table[] = {
         /* AVM BlueFRITZ! USB */
         { USB_DEVICE(0x057c, 0x2200) },
 
@@ -318,7 +318,6 @@ static inline int bfusb_recv_block(struct bfusb_data *data, int hdr, unsigned ch
                         return -ENOMEM;
                 }
 
-                skb->dev = (void *) data->hdev;
                 bt_cb(skb)->pkt_type = pkt_type;
 
                 data->reassembly = skb;
@@ -333,7 +332,7 @@ static inline int bfusb_recv_block(struct bfusb_data *data, int hdr, unsigned ch
                 memcpy(skb_put(data->reassembly, len), buf, len);
 
         if (hdr & 0x08) {
-                hci_recv_frame(data->reassembly);
+                hci_recv_frame(data->hdev, data->reassembly);
                 data->reassembly = NULL;
         }
 
@@ -465,26 +464,18 @@ static int bfusb_close(struct hci_dev *hdev)
         return 0;
 }
 
-static int bfusb_send_frame(struct sk_buff *skb)
+static int bfusb_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        struct hci_dev *hdev = (struct hci_dev *) skb->dev;
-        struct bfusb_data *data;
+        struct bfusb_data *data = hci_get_drvdata(hdev);
         struct sk_buff *nskb;
         unsigned char buf[3];
         int sent = 0, size, count;
 
         BT_DBG("hdev %p skb %p type %d len %d", hdev, skb, bt_cb(skb)->pkt_type, skb->len);
 
-        if (!hdev) {
-                BT_ERR("Frame for unknown HCI device (hdev=NULL)");
-                return -ENODEV;
-        }
-
         if (!test_bit(HCI_RUNNING, &hdev->flags))
                 return -EBUSY;
 
-        data = hci_get_drvdata(hdev);
-
         switch (bt_cb(skb)->pkt_type) {
         case HCI_COMMAND_PKT:
                 hdev->stat.cmd_tx++;
@@ -544,11 +535,6 @@ static int bfusb_send_frame(struct sk_buff *skb)
         return 0;
 }
 
-static int bfusb_ioctl(struct hci_dev *hdev, unsigned int cmd, unsigned long arg)
-{
-        return -ENOIOCTLCMD;
-}
-
 static int bfusb_load_firmware(struct bfusb_data *data,
                                const unsigned char *firmware, int count)
 {
@@ -699,11 +685,10 @@ static int bfusb_probe(struct usb_interface *intf, const struct usb_device_id *i
         hci_set_drvdata(hdev, data);
         SET_HCIDEV_DEV(hdev, &intf->dev);
 
-        hdev->open     = bfusb_open;
-        hdev->close    = bfusb_close;
-        hdev->flush    = bfusb_flush;
-        hdev->send     = bfusb_send_frame;
-        hdev->ioctl    = bfusb_ioctl;
+        hdev->open  = bfusb_open;
+        hdev->close = bfusb_close;
+        hdev->flush = bfusb_flush;
+        hdev->send  = bfusb_send_frame;
 
         if (hci_register_dev(hdev) < 0) {
                 BT_ERR("Can't register HCI device");
diff --git a/drivers/bluetooth/bluecard_cs.c b/drivers/bluetooth/bluecard_cs.c
index 6c3e3d43c718..57427de864a6 100644
--- a/drivers/bluetooth/bluecard_cs.c
+++ b/drivers/bluetooth/bluecard_cs.c
@@ -399,7 +399,6 @@ static void bluecard_receive(bluecard_info_t *info, unsigned int offset)
 
                 if (info->rx_state == RECV_WAIT_PACKET_TYPE) {
 
-                        info->rx_skb->dev = (void *) info->hdev;
                         bt_cb(info->rx_skb)->pkt_type = buf[i];
 
                         switch (bt_cb(info->rx_skb)->pkt_type) {
@@ -477,7 +476,7 @@ static void bluecard_receive(bluecard_info_t *info, unsigned int offset)
                                         break;
 
                                 case RECV_WAIT_DATA:
-                                        hci_recv_frame(info->rx_skb);
+                                        hci_recv_frame(info->hdev, info->rx_skb);
                                         info->rx_skb = NULL;
                                         break;
 
@@ -659,17 +658,9 @@ static int bluecard_hci_close(struct hci_dev *hdev)
 }
 
 
-static int bluecard_hci_send_frame(struct sk_buff *skb)
+static int bluecard_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        bluecard_info_t *info;
-        struct hci_dev *hdev = (struct hci_dev *)(skb->dev);
-
-        if (!hdev) {
-                BT_ERR("Frame for unknown HCI device (hdev=NULL)");
-                return -ENODEV;
-        }
-
-        info = hci_get_drvdata(hdev);
+        bluecard_info_t *info = hci_get_drvdata(hdev);
 
         switch (bt_cb(skb)->pkt_type) {
         case HCI_COMMAND_PKT:
@@ -693,12 +684,6 @@ static int bluecard_hci_send_frame(struct sk_buff *skb)
 }
 
 
-static int bluecard_hci_ioctl(struct hci_dev *hdev, unsigned int cmd, unsigned long arg)
-{
-        return -ENOIOCTLCMD;
-}
-
-
 
 /* ======================== Card services HCI interaction ======================== */
 
@@ -734,11 +719,10 @@ static int bluecard_open(bluecard_info_t *info)
         hci_set_drvdata(hdev, info);
         SET_HCIDEV_DEV(hdev, &info->p_dev->dev);
 
-        hdev->open     = bluecard_hci_open;
-        hdev->close    = bluecard_hci_close;
-        hdev->flush    = bluecard_hci_flush;
-        hdev->send     = bluecard_hci_send_frame;
-        hdev->ioctl    = bluecard_hci_ioctl;
+        hdev->open  = bluecard_hci_open;
+        hdev->close = bluecard_hci_close;
+        hdev->flush = bluecard_hci_flush;
+        hdev->send  = bluecard_hci_send_frame;
 
         id = inb(iobase + 0x30);
 
diff --git a/drivers/bluetooth/bpa10x.c b/drivers/bluetooth/bpa10x.c
index 2fe4a8031348..8a319913c9a9 100644
--- a/drivers/bluetooth/bpa10x.c
+++ b/drivers/bluetooth/bpa10x.c
@@ -37,7 +37,7 @@
 
 #define VERSION "0.10"
 
-static struct usb_device_id bpa10x_table[] = {
+static const struct usb_device_id bpa10x_table[] = {
         /* Tektronix BPA 100/105 (Digianswer) */
         { USB_DEVICE(0x08fd, 0x0002) },
 
@@ -129,8 +129,6 @@ static int bpa10x_recv(struct hci_dev *hdev, int queue, void *buf, int count)
                                 return -ENOMEM;
                         }
 
-                        skb->dev = (void *) hdev;
-
                         data->rx_skb[queue] = skb;
 
                         scb = (void *) skb->cb;
@@ -155,7 +153,7 @@ static int bpa10x_recv(struct hci_dev *hdev, int queue, void *buf, int count)
                         data->rx_skb[queue] = NULL;
 
                         bt_cb(skb)->pkt_type = scb->type;
-                        hci_recv_frame(skb);
+                        hci_recv_frame(hdev, skb);
                 }
 
                 count -= len; buf += len;
@@ -352,9 +350,8 @@ static int bpa10x_flush(struct hci_dev *hdev)
         return 0;
 }
 
-static int bpa10x_send_frame(struct sk_buff *skb)
+static int bpa10x_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        struct hci_dev *hdev = (struct hci_dev *) skb->dev;
         struct bpa10x_data *data = hci_get_drvdata(hdev);
         struct usb_ctrlrequest *dr;
         struct urb *urb;
@@ -366,6 +363,8 @@ static int bpa10x_send_frame(struct sk_buff *skb)
         if (!test_bit(HCI_RUNNING, &hdev->flags))
                 return -EBUSY;
 
+        skb->dev = (void *) hdev;
+
         urb = usb_alloc_urb(0, GFP_ATOMIC);
         if (!urb)
                 return -ENOMEM;
diff --git a/drivers/bluetooth/bt3c_cs.c b/drivers/bluetooth/bt3c_cs.c
index a1aaa3ba2a4b..73d87994d028 100644
--- a/drivers/bluetooth/bt3c_cs.c
+++ b/drivers/bluetooth/bt3c_cs.c
@@ -247,7 +247,6 @@ static void bt3c_receive(bt3c_info_t *info)
 
                 if (info->rx_state == RECV_WAIT_PACKET_TYPE) {
 
-                        info->rx_skb->dev = (void *) info->hdev;
                         bt_cb(info->rx_skb)->pkt_type = inb(iobase + DATA_L);
                         inb(iobase + DATA_H);
                         //printk("bt3c: PACKET_TYPE=%02x\n", bt_cb(info->rx_skb)->pkt_type);
@@ -318,7 +317,7 @@ static void bt3c_receive(bt3c_info_t *info)
                                         break;
 
                                 case RECV_WAIT_DATA:
-                                        hci_recv_frame(info->rx_skb);
+                                        hci_recv_frame(info->hdev, info->rx_skb);
                                         info->rx_skb = NULL;
                                         break;
 
@@ -416,19 +415,11 @@ static int bt3c_hci_close(struct hci_dev *hdev)
 }
 
 
-static int bt3c_hci_send_frame(struct sk_buff *skb)
+static int bt3c_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        bt3c_info_t *info;
-        struct hci_dev *hdev = (struct hci_dev *)(skb->dev);
+        bt3c_info_t *info = hci_get_drvdata(hdev);
         unsigned long flags;
 
-        if (!hdev) {
-                BT_ERR("Frame for unknown HCI device (hdev=NULL)");
-                return -ENODEV;
-        }
-
-        info = hci_get_drvdata(hdev);
-
         switch (bt_cb(skb)->pkt_type) {
         case HCI_COMMAND_PKT:
                 hdev->stat.cmd_tx++;
@@ -455,12 +446,6 @@ static int bt3c_hci_send_frame(struct sk_buff *skb)
 }
 
 
-static int bt3c_hci_ioctl(struct hci_dev *hdev, unsigned int cmd, unsigned long arg)
-{
-        return -ENOIOCTLCMD;
-}
-
-
 
 /* ======================== Card services HCI interaction ======================== */
 
@@ -577,11 +562,10 @@ static int bt3c_open(bt3c_info_t *info)
         hci_set_drvdata(hdev, info);
         SET_HCIDEV_DEV(hdev, &info->p_dev->dev);
 
-        hdev->open     = bt3c_hci_open;
-        hdev->close    = bt3c_hci_close;
-        hdev->flush    = bt3c_hci_flush;
-        hdev->send     = bt3c_hci_send_frame;
-        hdev->ioctl    = bt3c_hci_ioctl;
+        hdev->open  = bt3c_hci_open;
+        hdev->close = bt3c_hci_close;
+        hdev->flush = bt3c_hci_flush;
+        hdev->send  = bt3c_hci_send_frame;
 
         /* Load firmware */
         err = request_firmware(&firmware, "BT3CPCC.bin", &info->p_dev->dev);
diff --git a/drivers/bluetooth/btmrvl_main.c b/drivers/bluetooth/btmrvl_main.c
index 6e7bd4e4adbb..5cf31c4fe6d1 100644
--- a/drivers/bluetooth/btmrvl_main.c
+++ b/drivers/bluetooth/btmrvl_main.c
@@ -187,7 +187,6 @@ static int btmrvl_send_sync_cmd(struct btmrvl_private *priv, u16 cmd_no,
 
         bt_cb(skb)->pkt_type = MRVL_VENDOR_PKT;
 
-        skb->dev = (void *) priv->btmrvl_dev.hcidev;
         skb_queue_head(&priv->adapter->tx_queue, skb);
 
         priv->btmrvl_dev.sendcmdflag = true;
@@ -356,26 +355,12 @@ static void btmrvl_free_adapter(struct btmrvl_private *priv)
         priv->adapter = NULL;
 }
 
-static int btmrvl_ioctl(struct hci_dev *hdev,
-                                unsigned int cmd, unsigned long arg)
+static int btmrvl_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        return -ENOIOCTLCMD;
-}
-
-static int btmrvl_send_frame(struct sk_buff *skb)
-{
-        struct hci_dev *hdev = (struct hci_dev *) skb->dev;
-        struct btmrvl_private *priv = NULL;
+        struct btmrvl_private *priv = hci_get_drvdata(hdev);
 
         BT_DBG("type=%d, len=%d", skb->pkt_type, skb->len);
 
-        if (!hdev) {
-                BT_ERR("Frame for unknown HCI device");
-                return -ENODEV;
-        }
-
-        priv = hci_get_drvdata(hdev);
-
         if (!test_bit(HCI_RUNNING, &hdev->flags)) {
                 BT_ERR("Failed testing HCI_RUNING, flags=%lx", hdev->flags);
                 print_hex_dump_bytes("data: ", DUMP_PREFIX_OFFSET,
@@ -650,12 +635,11 @@ int btmrvl_register_hdev(struct btmrvl_private *priv)
         priv->btmrvl_dev.hcidev = hdev;
         hci_set_drvdata(hdev, priv);
 
-        hdev->bus = HCI_SDIO;
-        hdev->open = btmrvl_open;
+        hdev->bus   = HCI_SDIO;
+        hdev->open  = btmrvl_open;
         hdev->close = btmrvl_close;
         hdev->flush = btmrvl_flush;
-        hdev->send = btmrvl_send_frame;
-        hdev->ioctl = btmrvl_ioctl;
+        hdev->send  = btmrvl_send_frame;
         hdev->setup = btmrvl_setup;
 
         hdev->dev_type = priv->btmrvl_dev.dev_type;
diff --git a/drivers/bluetooth/btmrvl_sdio.c b/drivers/bluetooth/btmrvl_sdio.c
index 332475e400cf..fabcf5bb48af 100644
--- a/drivers/bluetooth/btmrvl_sdio.c
+++ b/drivers/bluetooth/btmrvl_sdio.c
@@ -600,15 +600,14 @@ static int btmrvl_sdio_card_to_host(struct btmrvl_private *priv)
         case HCI_SCODATA_PKT:
         case HCI_EVENT_PKT:
                 bt_cb(skb)->pkt_type = type;
-                skb->dev = (void *)hdev;
                 skb_put(skb, buf_len);
                 skb_pull(skb, SDIO_HEADER_LEN);
 
                 if (type == HCI_EVENT_PKT) {
                         if (btmrvl_check_evtpkt(priv, skb))
-                                hci_recv_frame(skb);
+                                hci_recv_frame(hdev, skb);
                 } else {
-                        hci_recv_frame(skb);
+                        hci_recv_frame(hdev, skb);
                 }
 
                 hdev->stat.byte_rx += buf_len;
@@ -616,12 +615,11 @@ static int btmrvl_sdio_card_to_host(struct btmrvl_private *priv)
 
         case MRVL_VENDOR_PKT:
                 bt_cb(skb)->pkt_type = HCI_VENDOR_PKT;
-                skb->dev = (void *)hdev;
                 skb_put(skb, buf_len);
                 skb_pull(skb, SDIO_HEADER_LEN);
 
                 if (btmrvl_process_event(priv, skb))
-                        hci_recv_frame(skb);
+                        hci_recv_frame(hdev, skb);
 
                 hdev->stat.byte_rx += buf_len;
                 break;
diff --git a/drivers/bluetooth/btsdio.c b/drivers/bluetooth/btsdio.c
index 4a9909713874..b61440aaee65 100644
--- a/drivers/bluetooth/btsdio.c
+++ b/drivers/bluetooth/btsdio.c
@@ -157,10 +157,9 @@ static int btsdio_rx_packet(struct btsdio_data *data)
 
         data->hdev->stat.byte_rx += len;
 
-        skb->dev = (void *) data->hdev;
         bt_cb(skb)->pkt_type = hdr[3];
 
-        err = hci_recv_frame(skb);
+        err = hci_recv_frame(data->hdev, skb);
         if (err < 0)
                 return err;
 
@@ -255,9 +254,8 @@ static int btsdio_flush(struct hci_dev *hdev)
         return 0;
 }
 
-static int btsdio_send_frame(struct sk_buff *skb)
+static int btsdio_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        struct hci_dev *hdev = (struct hci_dev *) skb->dev;
         struct btsdio_data *data = hci_get_drvdata(hdev);
 
         BT_DBG("%s", hdev->name);
diff --git a/drivers/bluetooth/btuart_cs.c b/drivers/bluetooth/btuart_cs.c
index beb262f2dc4d..a03ecc22a561 100644
--- a/drivers/bluetooth/btuart_cs.c
+++ b/drivers/bluetooth/btuart_cs.c
@@ -198,7 +198,6 @@ static void btuart_receive(btuart_info_t *info)
 
                 if (info->rx_state == RECV_WAIT_PACKET_TYPE) {
 
-                        info->rx_skb->dev = (void *) info->hdev;
                         bt_cb(info->rx_skb)->pkt_type = inb(iobase + UART_RX);
 
                         switch (bt_cb(info->rx_skb)->pkt_type) {
@@ -265,7 +264,7 @@ static void btuart_receive(btuart_info_t *info)
                                         break;
 
                                 case RECV_WAIT_DATA:
-                                        hci_recv_frame(info->rx_skb);
+                                        hci_recv_frame(info->hdev, info->rx_skb);
                                         info->rx_skb = NULL;
                                         break;
 
@@ -424,17 +423,9 @@ static int btuart_hci_close(struct hci_dev *hdev)
 }
 
 
-static int btuart_hci_send_frame(struct sk_buff *skb)
+static int btuart_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        btuart_info_t *info;
-        struct hci_dev *hdev = (struct hci_dev *)(skb->dev);
-
-        if (!hdev) {
-                BT_ERR("Frame for unknown HCI device (hdev=NULL)");
-                return -ENODEV;
-        }
-
-        info = hci_get_drvdata(hdev);
+        btuart_info_t *info = hci_get_drvdata(hdev);
 
         switch (bt_cb(skb)->pkt_type) {
         case HCI_COMMAND_PKT:
@@ -458,12 +449,6 @@ static int btuart_hci_send_frame(struct sk_buff *skb)
 }
 
 
-static int btuart_hci_ioctl(struct hci_dev *hdev, unsigned int cmd, unsigned long arg)
-{
-        return -ENOIOCTLCMD;
-}
-
-
 
 /* ======================== Card services HCI interaction ======================== */
 
@@ -495,11 +480,10 @@ static int btuart_open(btuart_info_t *info)
         hci_set_drvdata(hdev, info);
         SET_HCIDEV_DEV(hdev, &info->p_dev->dev);
 
-        hdev->open     = btuart_hci_open;
-        hdev->close    = btuart_hci_close;
-        hdev->flush    = btuart_hci_flush;
-        hdev->send     = btuart_hci_send_frame;
-        hdev->ioctl    = btuart_hci_ioctl;
+        hdev->open  = btuart_hci_open;
+        hdev->close = btuart_hci_close;
+        hdev->flush = btuart_hci_flush;
+        hdev->send  = btuart_hci_send_frame;
 
         spin_lock_irqsave(&(info->lock), flags);
 
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index f3dfc0a88fdc..30868fa870f6 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -50,7 +50,7 @@ static struct usb_driver btusb_driver;
 #define BTUSB_ATH3012           0x80
 #define BTUSB_INTEL             0x100
 
-static struct usb_device_id btusb_table[] = {
+static const struct usb_device_id btusb_table[] = {
         /* Generic Bluetooth USB device */
         { USB_DEVICE_INFO(0xe0, 0x01, 0x01) },
 
@@ -121,7 +121,7 @@ static struct usb_device_id btusb_table[] = {
 
 MODULE_DEVICE_TABLE(usb, btusb_table);
 
-static struct usb_device_id blacklist_table[] = {
+static const struct usb_device_id blacklist_table[] = {
         /* CSR BlueCore devices */
         { USB_DEVICE(0x0a12, 0x0001), .driver_info = BTUSB_CSR },
 
@@ -716,9 +716,8 @@ static int btusb_flush(struct hci_dev *hdev)
         return 0;
 }
 
-static int btusb_send_frame(struct sk_buff *skb)
+static int btusb_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        struct hci_dev *hdev = (struct hci_dev *) skb->dev;
         struct btusb_data *data = hci_get_drvdata(hdev);
         struct usb_ctrlrequest *dr;
         struct urb *urb;
@@ -730,6 +729,8 @@ static int btusb_send_frame(struct sk_buff *skb)
         if (!test_bit(HCI_RUNNING, &hdev->flags))
                 return -EBUSY;
 
+        skb->dev = (void *) hdev;
+
         switch (bt_cb(skb)->pkt_type) {
         case HCI_COMMAND_PKT:
                 urb = usb_alloc_urb(0, GFP_ATOMIC);
@@ -774,7 +775,7 @@ static int btusb_send_frame(struct sk_buff *skb)
                 break;
 
         case HCI_SCODATA_PKT:
-                if (!data->isoc_tx_ep || hdev->conn_hash.sco_num < 1)
+                if (!data->isoc_tx_ep || hci_conn_num(hdev, SCO_LINK) < 1)
                         return -ENODEV;
 
                 urb = usb_alloc_urb(BTUSB_MAX_ISOC_FRAMES, GFP_ATOMIC);
@@ -833,8 +834,8 @@ static void btusb_notify(struct hci_dev *hdev, unsigned int evt)
 
         BT_DBG("%s evt %d", hdev->name, evt);
 
-        if (hdev->conn_hash.sco_num != data->sco_num) {
-                data->sco_num = hdev->conn_hash.sco_num;
+        if (hci_conn_num(hdev, SCO_LINK) != data->sco_num) {
+                data->sco_num = hci_conn_num(hdev, SCO_LINK);
                 schedule_work(&data->work);
         }
 }
@@ -889,7 +890,7 @@ static void btusb_work(struct work_struct *work)
         int new_alts;
         int err;
 
-        if (hdev->conn_hash.sco_num > 0) {
+        if (data->sco_num > 0) {
                 if (!test_bit(BTUSB_DID_ISO_RESUME, &data->flags)) {
                         err = usb_autopm_get_interface(data->isoc ? data->isoc : data->intf);
                         if (err < 0) {
@@ -903,9 +904,9 @@ static void btusb_work(struct work_struct *work)
 
                 if (hdev->voice_setting & 0x0020) {
                         static const int alts[3] = { 2, 4, 5 };
-                        new_alts = alts[hdev->conn_hash.sco_num - 1];
+                        new_alts = alts[data->sco_num - 1];
                 } else {
-                        new_alts = hdev->conn_hash.sco_num;
+                        new_alts = data->sco_num;
                 }
 
                 if (data->isoc_altsetting != new_alts) {
diff --git a/drivers/bluetooth/btwilink.c b/drivers/bluetooth/btwilink.c
index 60abf596f60e..f038dba19e36 100644
--- a/drivers/bluetooth/btwilink.c
+++ b/drivers/bluetooth/btwilink.c
@@ -108,10 +108,8 @@ static long st_receive(void *priv_data, struct sk_buff *skb)
                 return -EFAULT;
         }
 
-        skb->dev = (void *) lhst->hdev;
-
         /* Forward skb to HCI core layer */
-        err = hci_recv_frame(skb);
+        err = hci_recv_frame(lhst->hdev, skb);
         if (err < 0) {
                 BT_ERR("Unable to push skb to HCI core(%d)", err);
                 return err;
@@ -253,14 +251,11 @@ static int ti_st_close(struct hci_dev *hdev)
         return err;
 }
 
-static int ti_st_send_frame(struct sk_buff *skb)
+static int ti_st_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        struct hci_dev *hdev;
         struct ti_st *hst;
         long len;
 
-        hdev = (struct hci_dev *)skb->dev;
-
         if (!test_bit(HCI_RUNNING, &hdev->flags))
                 return -EBUSY;
 
diff --git a/drivers/bluetooth/dtl1_cs.c b/drivers/bluetooth/dtl1_cs.c
index 33f3a6950c0e..52eed1f3565d 100644
--- a/drivers/bluetooth/dtl1_cs.c
+++ b/drivers/bluetooth/dtl1_cs.c
@@ -256,9 +256,8 @@ static void dtl1_receive(dtl1_info_t *info)
                                 case 0x83:
                                 case 0x84:
                                         /* send frame to the HCI layer */
-                                        info->rx_skb->dev = (void *) info->hdev;
                                         bt_cb(info->rx_skb)->pkt_type &= 0x0f;
-                                        hci_recv_frame(info->rx_skb);
+                                        hci_recv_frame(info->hdev, info->rx_skb);
                                         break;
                                 default:
                                         /* unknown packet */
@@ -383,20 +382,12 @@ static int dtl1_hci_close(struct hci_dev *hdev)
 }
 
 
-static int dtl1_hci_send_frame(struct sk_buff *skb)
+static int dtl1_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        dtl1_info_t *info;
-        struct hci_dev *hdev = (struct hci_dev *)(skb->dev);
+        dtl1_info_t *info = hci_get_drvdata(hdev);
         struct sk_buff *s;
         nsh_t nsh;
 
-        if (!hdev) {
-                BT_ERR("Frame for unknown HCI device (hdev=NULL)");
-                return -ENODEV;
-        }
-
-        info = hci_get_drvdata(hdev);
-
         switch (bt_cb(skb)->pkt_type) {
         case HCI_COMMAND_PKT:
                 hdev->stat.cmd_tx++;
@@ -438,12 +429,6 @@ static int dtl1_hci_send_frame(struct sk_buff *skb)
 }
 
 
-static int dtl1_hci_ioctl(struct hci_dev *hdev, unsigned int cmd,  unsigned long arg)
-{
-        return -ENOIOCTLCMD;
-}
-
-
 
 /* ======================== Card services HCI interaction ======================== */
 
@@ -477,11 +462,10 @@ static int dtl1_open(dtl1_info_t *info)
         hci_set_drvdata(hdev, info);
         SET_HCIDEV_DEV(hdev, &info->p_dev->dev);
 
-        hdev->open     = dtl1_hci_open;
-        hdev->close    = dtl1_hci_close;
-        hdev->flush    = dtl1_hci_flush;
-        hdev->send     = dtl1_hci_send_frame;
-        hdev->ioctl    = dtl1_hci_ioctl;
+        hdev->open  = dtl1_hci_open;
+        hdev->close = dtl1_hci_close;
+        hdev->flush = dtl1_hci_flush;
+        hdev->send  = dtl1_hci_send_frame;
 
         spin_lock_irqsave(&(info->lock), flags);
 
diff --git a/drivers/bluetooth/hci_bcsp.c b/drivers/bluetooth/hci_bcsp.c
index 57e502e06080..0bc87f7abd95 100644
--- a/drivers/bluetooth/hci_bcsp.c
+++ b/drivers/bluetooth/hci_bcsp.c
@@ -522,7 +522,7 @@ static void bcsp_complete_rx_pkt(struct hci_uart *hu)
                                 memcpy(skb_push(bcsp->rx_skb, HCI_EVENT_HDR_SIZE), &hdr, HCI_EVENT_HDR_SIZE);
                                 bt_cb(bcsp->rx_skb)->pkt_type = HCI_EVENT_PKT;
 
-                                hci_recv_frame(bcsp->rx_skb);
+                                hci_recv_frame(hu->hdev, bcsp->rx_skb);
                         } else {
                                 BT_ERR ("Packet for unknown channel (%u %s)",
                                         bcsp->rx_skb->data[1] & 0x0f,
@@ -536,7 +536,7 @@ static void bcsp_complete_rx_pkt(struct hci_uart *hu)
                 /* Pull out BCSP hdr */
                 skb_pull(bcsp->rx_skb, 4);
 
-                hci_recv_frame(bcsp->rx_skb);
+                hci_recv_frame(hu->hdev, bcsp->rx_skb);
         }
 
         bcsp->rx_state = BCSP_W4_PKT_DELIMITER;
@@ -655,7 +655,6 @@ static int bcsp_recv(struct hci_uart *hu, void *data, int count)
                                         bcsp->rx_count = 0;
                                         return 0;
                                 }
-                                bcsp->rx_skb->dev = (void *) hu->hdev;
                                 break;
                         }
                         break;
diff --git a/drivers/bluetooth/hci_h4.c b/drivers/bluetooth/hci_h4.c
index 8ae9f1ea2bb5..7048a583fe51 100644
--- a/drivers/bluetooth/hci_h4.c
+++ b/drivers/bluetooth/hci_h4.c
@@ -124,30 +124,6 @@ static int h4_enqueue(struct hci_uart *hu, struct sk_buff *skb)
         return 0;
 }
 
-static inline int h4_check_data_len(struct h4_struct *h4, int len)
-{
-        int room = skb_tailroom(h4->rx_skb);
-
-        BT_DBG("len %d room %d", len, room);
-
-        if (!len) {
-                hci_recv_frame(h4->rx_skb);
-        } else if (len > room) {
-                BT_ERR("Data length is too large");
-                kfree_skb(h4->rx_skb);
-        } else {
-                h4->rx_state = H4_W4_DATA;
-                h4->rx_count = len;
-                return len;
-        }
-
-        h4->rx_state = H4_W4_PACKET_TYPE;
-        h4->rx_skb   = NULL;
-        h4->rx_count = 0;
-
-        return 0;
-}
-
 /* Recv data */
 static int h4_recv(struct hci_uart *hu, void *data, int count)
 {
diff --git a/drivers/bluetooth/hci_h5.c b/drivers/bluetooth/hci_h5.c
index b6154d5a07a5..f6f497450560 100644
--- a/drivers/bluetooth/hci_h5.c
+++ b/drivers/bluetooth/hci_h5.c
@@ -340,7 +340,7 @@ static void h5_complete_rx_pkt(struct hci_uart *hu)
                 /* Remove Three-wire header */
                 skb_pull(h5->rx_skb, 4);
 
-                hci_recv_frame(h5->rx_skb);
+                hci_recv_frame(hu->hdev, h5->rx_skb);
                 h5->rx_skb = NULL;
 
                 break;
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index bc68a440d432..6e06f6f69152 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -234,21 +234,13 @@ static int hci_uart_close(struct hci_dev *hdev)
 }
 
 /* Send frames from HCI layer */
-static int hci_uart_send_frame(struct sk_buff *skb)
+static int hci_uart_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        struct hci_dev* hdev = (struct hci_dev *) skb->dev;
-        struct hci_uart *hu;
-
-        if (!hdev) {
-                BT_ERR("Frame for unknown device (hdev=NULL)");
-                return -ENODEV;
-        }
+        struct hci_uart *hu = hci_get_drvdata(hdev);
 
         if (!test_bit(HCI_RUNNING, &hdev->flags))
                 return -EBUSY;
 
-        hu = hci_get_drvdata(hdev);
-
         BT_DBG("%s: type %d len %d", hdev->name, bt_cb(skb)->pkt_type, skb->len);
 
         hu->proto->enqueue(hu, skb);
diff --git a/drivers/bluetooth/hci_ll.c b/drivers/bluetooth/hci_ll.c
index cfc767938589..69a90b1b5ff5 100644
--- a/drivers/bluetooth/hci_ll.c
+++ b/drivers/bluetooth/hci_ll.c
@@ -110,7 +110,6 @@ static int send_hcill_cmd(u8 cmd, struct hci_uart *hu)
         /* prepare packet */
         hcill_packet = (struct hcill_cmd *) skb_put(skb, 1);
         hcill_packet->cmd = cmd;
-        skb->dev = (void *) hu->hdev;
 
         /* send packet */
         skb_queue_tail(&ll->txq, skb);
@@ -346,14 +345,14 @@ static int ll_enqueue(struct hci_uart *hu, struct sk_buff *skb)
         return 0;
 }
 
-static inline int ll_check_data_len(struct ll_struct *ll, int len)
+static inline int ll_check_data_len(struct hci_dev *hdev, struct ll_struct *ll, int len)
 {
         int room = skb_tailroom(ll->rx_skb);
 
         BT_DBG("len %d room %d", len, room);
 
         if (!len) {
-                hci_recv_frame(ll->rx_skb);
+                hci_recv_frame(hdev, ll->rx_skb);
         } else if (len > room) {
                 BT_ERR("Data length is too large");
                 kfree_skb(ll->rx_skb);
@@ -395,7 +394,7 @@ static int ll_recv(struct hci_uart *hu, void *data, int count)
                         switch (ll->rx_state) {
                         case HCILL_W4_DATA:
                                 BT_DBG("Complete data");
-                                hci_recv_frame(ll->rx_skb);
+                                hci_recv_frame(hu->hdev, ll->rx_skb);
 
                                 ll->rx_state = HCILL_W4_PACKET_TYPE;
                                 ll->rx_skb = NULL;
@@ -406,7 +405,7 @@ static int ll_recv(struct hci_uart *hu, void *data, int count)
 
                                 BT_DBG("Event header: evt 0x%2.2x plen %d", eh->evt, eh->plen);
 
-                                ll_check_data_len(ll, eh->plen);
+                                ll_check_data_len(hu->hdev, ll, eh->plen);
                                 continue;
 
                         case HCILL_W4_ACL_HDR:
@@ -415,7 +414,7 @@ static int ll_recv(struct hci_uart *hu, void *data, int count)
 
                                 BT_DBG("ACL header: dlen %d", dlen);
 
-                                ll_check_data_len(ll, dlen);
+                                ll_check_data_len(hu->hdev, ll, dlen);
                                 continue;
 
                         case HCILL_W4_SCO_HDR:
@@ -423,7 +422,7 @@ static int ll_recv(struct hci_uart *hu, void *data, int count)
 
                                 BT_DBG("SCO header: dlen %d", sh->dlen);
 
-                                ll_check_data_len(ll, sh->dlen);
+                                ll_check_data_len(hu->hdev, ll, sh->dlen);
                                 continue;
                         }
                 }
@@ -494,7 +493,6 @@ static int ll_recv(struct hci_uart *hu, void *data, int count)
                         return -ENOMEM;
                 }
 
-                ll->rx_skb->dev = (void *) hu->hdev;
                 bt_cb(ll->rx_skb)->pkt_type = type;
         }
 
diff --git a/drivers/bluetooth/hci_vhci.c b/drivers/bluetooth/hci_vhci.c
index c04a3e6fb37c..7b167385a1c4 100644
--- a/drivers/bluetooth/hci_vhci.c
+++ b/drivers/bluetooth/hci_vhci.c
@@ -81,21 +81,13 @@ static int vhci_flush(struct hci_dev *hdev)
         return 0;
 }
 
-static int vhci_send_frame(struct sk_buff *skb)
+static int vhci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        struct hci_dev* hdev = (struct hci_dev *) skb->dev;
-        struct vhci_data *data;
-
-        if (!hdev) {
-                BT_ERR("Frame for unknown HCI device (hdev=NULL)");
-                return -ENODEV;
-        }
+        struct vhci_data *data = hci_get_drvdata(hdev);
 
         if (!test_bit(HCI_RUNNING, &hdev->flags))
                 return -EBUSY;
 
-        data = hci_get_drvdata(hdev);
-
         memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
         skb_queue_tail(&data->readq, skb);
 
@@ -179,10 +171,9 @@ static inline ssize_t vhci_get_user(struct vhci_data *data,
                         return -ENODEV;
                 }
 
-                skb->dev = (void *) data->hdev;
                 bt_cb(skb)->pkt_type = pkt_type;
 
-                ret = hci_recv_frame(skb);
+                ret = hci_recv_frame(data->hdev, skb);
                 break;
 
         case HCI_VENDOR_PKT:
diff --git a/drivers/net/wireless/ath/Kconfig b/drivers/net/wireless/ath/Kconfig
index ba81d6292eeb..c63d1159db5c 100644
--- a/drivers/net/wireless/ath/Kconfig
+++ b/drivers/net/wireless/ath/Kconfig
@@ -25,6 +25,23 @@ config ATH_DEBUG
           Say Y, if you want to debug atheros wireless drivers.
           Right now only ath9k makes use of this.
 
+config ATH_REG_DYNAMIC_USER_REG_HINTS
+        bool "Atheros dynamic user regulatory hints"
+        depends on CFG80211_CERTIFICATION_ONUS
+        default n
+        ---help---
+          Say N. This should only be enabled in countries where
+          this feature is explicitly allowed and only on cards that
+          specifically have been tested for this.
+
+config ATH_REG_DYNAMIC_USER_CERT_TESTING
+        bool "Atheros dynamic user regulatory testing"
+        depends on ATH_REG_DYNAMIC_USER_REG_HINTS && CFG80211_CERTIFICATION_ONUS
+        default n
+        ---help---
+          Say N. This should only be enabled on systems
+          undergoing certification testing.
+
 source "drivers/net/wireless/ath/ath5k/Kconfig"
 source "drivers/net/wireless/ath/ath9k/Kconfig"
 source "drivers/net/wireless/ath/carl9170/Kconfig"
diff --git a/drivers/net/wireless/ath/Makefile b/drivers/net/wireless/ath/Makefile
index 363b05653c7e..7d023b0f13b4 100644
--- a/drivers/net/wireless/ath/Makefile
+++ b/drivers/net/wireless/ath/Makefile
@@ -12,7 +12,9 @@ obj-$(CONFIG_ATH_COMMON)	+= ath.o
 ath-objs :=     main.o \
                 regd.o \
                 hw.o \
-                key.o
+                key.o \
+                dfs_pattern_detector.o \
+                dfs_pri_detector.o
 
 ath-$(CONFIG_ATH_DEBUG) += debug.o
 ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/ath/ath10k/ce.c b/drivers/net/wireless/ath/ath10k/ce.c
index 834e29ea236c..e46951b8fb92 100644
--- a/drivers/net/wireless/ath/ath10k/ce.c
+++ b/drivers/net/wireless/ath/ath10k/ce.c
@@ -283,7 +283,7 @@ static int ath10k_ce_send_nolock(struct ath10k_ce_pipe *ce_state,
 
         if (unlikely(CE_RING_DELTA(nentries_mask,
                                    write_index, sw_index - 1) <= 0)) {
-                ret = -EIO;
+                ret = -ENOSR;
                 goto exit;
         }
 
@@ -338,38 +338,19 @@ int ath10k_ce_send(struct ath10k_ce_pipe *ce_state,
         return ret;
 }
 
-int ath10k_ce_sendlist_send(struct ath10k_ce_pipe *ce_state,
-                            void *per_transfer_context,
-                            unsigned int transfer_id,
-                            u32 paddr, unsigned int nbytes,
-                            u32 flags)
+int ath10k_ce_num_free_src_entries(struct ath10k_ce_pipe *pipe)
 {
-        struct ath10k_ce_ring *src_ring = ce_state->src_ring;
-        struct ath10k *ar = ce_state->ar;
+        struct ath10k *ar = pipe->ar;
         struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-        unsigned int nentries_mask = src_ring->nentries_mask;
-        unsigned int sw_index;
-        unsigned int write_index;
-        int delta, ret = -ENOMEM;
+        int delta;
 
         spin_lock_bh(&ar_pci->ce_lock);
-
-        sw_index = src_ring->sw_index;
-        write_index = src_ring->write_index;
-
-        delta = CE_RING_DELTA(nentries_mask, write_index, sw_index - 1);
-
-        if (delta >= 1) {
-                ret = ath10k_ce_send_nolock(ce_state, per_transfer_context,
-                                            paddr, nbytes,
-                                            transfer_id, flags);
-                if (ret)
-                        ath10k_warn("CE send failed: %d\n", ret);
-        }
-
+        delta = CE_RING_DELTA(pipe->src_ring->nentries_mask,
+                              pipe->src_ring->write_index,
+                              pipe->src_ring->sw_index - 1);
         spin_unlock_bh(&ar_pci->ce_lock);
 
-        return ret;
+        return delta;
 }
 
 int ath10k_ce_recv_buf_enqueue(struct ath10k_ce_pipe *ce_state,
diff --git a/drivers/net/wireless/ath/ath10k/ce.h b/drivers/net/wireless/ath/ath10k/ce.h
index aec802868341..15d45b5b7615 100644
--- a/drivers/net/wireless/ath/ath10k/ce.h
+++ b/drivers/net/wireless/ath/ath10k/ce.h
@@ -156,21 +156,7 @@ void ath10k_ce_send_cb_register(struct ath10k_ce_pipe *ce_state,
                                 void (*send_cb)(struct ath10k_ce_pipe *),
                                 int disable_interrupts);
 
-/*
- * Queue a "sendlist" of buffers to be sent using gather to a single
- * anonymous destination buffer
- *   ce         - which copy engine to use
- *   sendlist        - list of simple buffers to send using gather
- *   transfer_id     - arbitrary ID; reflected to destination
- * Returns 0 on success; otherwise an error status.
- *
- * Implemenation note: Pushes multiple buffers with Gather to Source ring.
- */
-int ath10k_ce_sendlist_send(struct ath10k_ce_pipe *ce_state,
-                            void *per_transfer_context,
-                            unsigned int transfer_id,
-                            u32 paddr, unsigned int nbytes,
-                            u32 flags);
+int ath10k_ce_num_free_src_entries(struct ath10k_ce_pipe *pipe);
 
 /*==================Recv=======================*/
 
diff --git a/drivers/net/wireless/ath/ath10k/core.c b/drivers/net/wireless/ath/ath10k/core.c
index 76906d5a082e..1129994fb105 100644
--- a/drivers/net/wireless/ath/ath10k/core.c
+++ b/drivers/net/wireless/ath/ath10k/core.c
@@ -59,27 +59,6 @@ static void ath10k_send_suspend_complete(struct ath10k *ar)
         wake_up(&ar->event_queue);
 }
 
-static int ath10k_check_fw_version(struct ath10k *ar)
-{
-        char version[32];
-
-        if (ar->fw_version_major >= SUPPORTED_FW_MAJOR &&
-            ar->fw_version_minor >= SUPPORTED_FW_MINOR &&
-            ar->fw_version_release >= SUPPORTED_FW_RELEASE &&
-            ar->fw_version_build >= SUPPORTED_FW_BUILD)
-                return 0;
-
-        snprintf(version, sizeof(version), "%u.%u.%u.%u",
-                 SUPPORTED_FW_MAJOR, SUPPORTED_FW_MINOR,
-                 SUPPORTED_FW_RELEASE, SUPPORTED_FW_BUILD);
-
-        ath10k_warn("WARNING: Firmware version %s is not officially supported.\n",
-                    ar->hw->wiphy->fw_version);
-        ath10k_warn("Please upgrade to version %s (or newer)\n", version);
-
-        return 0;
-}
-
 static int ath10k_init_connect_htc(struct ath10k *ar)
 {
         int status;
@@ -189,8 +168,7 @@ static const struct firmware *ath10k_fetch_fw_file(struct ath10k *ar,
         return fw;
 }
 
-static int ath10k_push_board_ext_data(struct ath10k *ar,
-                                      const struct firmware *fw)
+static int ath10k_push_board_ext_data(struct ath10k *ar)
 {
         u32 board_data_size = QCA988X_BOARD_DATA_SZ;
         u32 board_ext_data_size = QCA988X_BOARD_EXT_DATA_SZ;
@@ -210,14 +188,14 @@ static int ath10k_push_board_ext_data(struct ath10k *ar,
         if (board_ext_data_addr == 0)
                 return 0;
 
-        if (fw->size != (board_data_size + board_ext_data_size)) {
+        if (ar->board_len != (board_data_size + board_ext_data_size)) {
                 ath10k_err("invalid board (ext) data sizes %zu != %d+%d\n",
-                           fw->size, board_data_size, board_ext_data_size);
+                           ar->board_len, board_data_size, board_ext_data_size);
                 return -EINVAL;
         }
 
         ret = ath10k_bmi_write_memory(ar, board_ext_data_addr,
-                                      fw->data + board_data_size,
+                                      ar->board_data + board_data_size,
                                       board_ext_data_size);
         if (ret) {
                 ath10k_err("could not write board ext data (%d)\n", ret);
@@ -236,12 +214,11 @@ static int ath10k_push_board_ext_data(struct ath10k *ar,
 
 static int ath10k_download_board_data(struct ath10k *ar)
 {
-        const struct firmware *fw = ar->board_data;
         u32 board_data_size = QCA988X_BOARD_DATA_SZ;
         u32 address;
         int ret;
 
-        ret = ath10k_push_board_ext_data(ar, fw);
+        ret = ath10k_push_board_ext_data(ar);
         if (ret) {
                 ath10k_err("could not push board ext data (%d)\n", ret);
                 goto exit;
@@ -253,8 +230,9 @@ static int ath10k_download_board_data(struct ath10k *ar)
                 goto exit;
         }
 
-        ret = ath10k_bmi_write_memory(ar, address, fw->data,
-                                      min_t(u32, board_data_size, fw->size));
+        ret = ath10k_bmi_write_memory(ar, address, ar->board_data,
+                                      min_t(u32, board_data_size,
+                                            ar->board_len));
         if (ret) {
                 ath10k_err("could not write board data (%d)\n", ret);
                 goto exit;
@@ -272,17 +250,16 @@ exit:
 
 static int ath10k_download_and_run_otp(struct ath10k *ar)
 {
-        const struct firmware *fw = ar->otp;
         u32 address = ar->hw_params.patch_load_addr;
         u32 exec_param;
         int ret;
 
         /* OTP is optional */
 
-        if (!ar->otp)
+        if (!ar->otp_data || !ar->otp_len)
                 return 0;
 
-        ret = ath10k_bmi_fast_download(ar, address, fw->data, fw->size);
+        ret = ath10k_bmi_fast_download(ar, address, ar->otp_data, ar->otp_len);
         if (ret) {
                 ath10k_err("could not write otp (%d)\n", ret);
                 goto exit;
@@ -301,13 +278,13 @@ exit:
 
 static int ath10k_download_fw(struct ath10k *ar)
 {
-        const struct firmware *fw = ar->firmware;
         u32 address;
         int ret;
 
         address = ar->hw_params.patch_load_addr;
 
-        ret = ath10k_bmi_fast_download(ar, address, fw->data, fw->size);
+        ret = ath10k_bmi_fast_download(ar, address, ar->firmware_data,
+                                       ar->firmware_len);
         if (ret) {
                 ath10k_err("could not write fw (%d)\n", ret);
                 goto exit;
@@ -319,8 +296,8 @@ exit:
 
 static void ath10k_core_free_firmware_files(struct ath10k *ar)
 {
-        if (ar->board_data && !IS_ERR(ar->board_data))
-                release_firmware(ar->board_data);
+        if (ar->board && !IS_ERR(ar->board))
+                release_firmware(ar->board);
 
         if (ar->otp && !IS_ERR(ar->otp))
                 release_firmware(ar->otp);
@@ -328,12 +305,20 @@ static void ath10k_core_free_firmware_files(struct ath10k *ar)
         if (ar->firmware && !IS_ERR(ar->firmware))
                 release_firmware(ar->firmware);
 
+        ar->board = NULL;
         ar->board_data = NULL;
+        ar->board_len = 0;
+
         ar->otp = NULL;
+        ar->otp_data = NULL;
+        ar->otp_len = 0;
+
         ar->firmware = NULL;
+        ar->firmware_data = NULL;
+        ar->firmware_len = 0;
 }
 
-static int ath10k_core_fetch_firmware_files(struct ath10k *ar)
+static int ath10k_core_fetch_firmware_api_1(struct ath10k *ar)
 {
         int ret = 0;
 
@@ -347,15 +332,18 @@ static int ath10k_core_fetch_firmware_files(struct ath10k *ar)
                 return -EINVAL;
         }
 
-        ar->board_data = ath10k_fetch_fw_file(ar,
-                                              ar->hw_params.fw.dir,
-                                              ar->hw_params.fw.board);
-        if (IS_ERR(ar->board_data)) {
-                ret = PTR_ERR(ar->board_data);
+        ar->board = ath10k_fetch_fw_file(ar,
+                                         ar->hw_params.fw.dir,
+                                         ar->hw_params.fw.board);
+        if (IS_ERR(ar->board)) {
+                ret = PTR_ERR(ar->board);
                 ath10k_err("could not fetch board data (%d)\n", ret);
                 goto err;
         }
 
+        ar->board_data = ar->board->data;
+        ar->board_len = ar->board->size;
+
         ar->firmware = ath10k_fetch_fw_file(ar,
                                             ar->hw_params.fw.dir,
                                             ar->hw_params.fw.fw);
@@ -365,6 +353,9 @@ static int ath10k_core_fetch_firmware_files(struct ath10k *ar)
                 goto err;
         }
 
+        ar->firmware_data = ar->firmware->data;
+        ar->firmware_len = ar->firmware->size;
+
         /* OTP may be undefined. If so, don't fetch it at all */
         if (ar->hw_params.fw.otp == NULL)
                 return 0;
@@ -378,6 +369,9 @@ static int ath10k_core_fetch_firmware_files(struct ath10k *ar)
                 goto err;
         }
 
+        ar->otp_data = ar->otp->data;
+        ar->otp_len = ar->otp->size;
+
         return 0;
 
 err:
@@ -385,6 +379,191 @@ err:
         return ret;
 }
 
+static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
+{
+        size_t magic_len, len, ie_len;
+        int ie_id, i, index, bit, ret;
+        struct ath10k_fw_ie *hdr;
+        const u8 *data;
+        __le32 *timestamp;
+
+        /* first fetch the firmware file (firmware-*.bin) */
+        ar->firmware = ath10k_fetch_fw_file(ar, ar->hw_params.fw.dir, name);
+        if (IS_ERR(ar->firmware)) {
+                ath10k_err("Could not fetch firmware file '%s': %ld\n",
+                           name, PTR_ERR(ar->firmware));
+                return PTR_ERR(ar->firmware);
+        }
+
+        data = ar->firmware->data;
+        len = ar->firmware->size;
+
+        /* magic also includes the null byte, check that as well */
+        magic_len = strlen(ATH10K_FIRMWARE_MAGIC) + 1;
+
+        if (len < magic_len) {
+                ath10k_err("firmware image too small to contain magic: %zu\n",
+                           len);
+                ret = -EINVAL;
+                goto err;
+        }
+
+        if (memcmp(data, ATH10K_FIRMWARE_MAGIC, magic_len) != 0) {
+                ath10k_err("Invalid firmware magic\n");
+                ret = -EINVAL;
+                goto err;
+        }
+
+        /* jump over the padding */
+        magic_len = ALIGN(magic_len, 4);
+
+        len -= magic_len;
+        data += magic_len;
+
+        /* loop elements */
+        while (len > sizeof(struct ath10k_fw_ie)) {
+                hdr = (struct ath10k_fw_ie *)data;
+
+                ie_id = le32_to_cpu(hdr->id);
+                ie_len = le32_to_cpu(hdr->len);
+
+                len -= sizeof(*hdr);
+                data += sizeof(*hdr);
+
+                if (len < ie_len) {
+                        ath10k_err("Invalid length for FW IE %d (%zu < %zu)\n",
+                                   ie_id, len, ie_len);
+                        ret = -EINVAL;
+                        goto err;
+                }
+
+                switch (ie_id) {
+                case ATH10K_FW_IE_FW_VERSION:
+                        if (ie_len > sizeof(ar->hw->wiphy->fw_version) - 1)
+                                break;
+
+                        memcpy(ar->hw->wiphy->fw_version, data, ie_len);
+                        ar->hw->wiphy->fw_version[ie_len] = '\0';
+
+                        ath10k_dbg(ATH10K_DBG_BOOT,
+                                   "found fw version %s\n",
+                                    ar->hw->wiphy->fw_version);
+                        break;
+                case ATH10K_FW_IE_TIMESTAMP:
+                        if (ie_len != sizeof(u32))
+                                break;
+
+                        timestamp = (__le32 *)data;
+
+                        ath10k_dbg(ATH10K_DBG_BOOT, "found fw timestamp %d\n",
+                                   le32_to_cpup(timestamp));
+                        break;
+                case ATH10K_FW_IE_FEATURES:
+                        ath10k_dbg(ATH10K_DBG_BOOT,
+                                   "found firmware features ie (%zd B)\n",
+                                   ie_len);
+
+                        for (i = 0; i < ATH10K_FW_FEATURE_COUNT; i++) {
+                                index = i / 8;
+                                bit = i % 8;
+
+                                if (index == ie_len)
+                                        break;
+
+                                if (data[index] & (1 << bit))
+                                        __set_bit(i, ar->fw_features);
+                        }
+
+                        ath10k_dbg_dump(ATH10K_DBG_BOOT, "features", "",
+                                        ar->fw_features,
+                                        sizeof(ar->fw_features));
+                        break;
+                case ATH10K_FW_IE_FW_IMAGE:
+                        ath10k_dbg(ATH10K_DBG_BOOT,
+                                   "found fw image ie (%zd B)\n",
+                                   ie_len);
+
+                        ar->firmware_data = data;
+                        ar->firmware_len = ie_len;
+
+                        break;
+                case ATH10K_FW_IE_OTP_IMAGE:
+                        ath10k_dbg(ATH10K_DBG_BOOT,
+                                   "found otp image ie (%zd B)\n",
+                                   ie_len);
+
+                        ar->otp_data = data;
+                        ar->otp_len = ie_len;
+
+                        break;
+                default:
+                        ath10k_warn("Unknown FW IE: %u\n",
+                                    le32_to_cpu(hdr->id));
+                        break;
+                }
+
+                /* jump over the padding */
+                ie_len = ALIGN(ie_len, 4);
+
+                len -= ie_len;
+                data += ie_len;
+        }
+
+        if (!ar->firmware_data || !ar->firmware_len) {
+                ath10k_warn("No ATH10K_FW_IE_FW_IMAGE found from %s, skipping\n",
+                            name);
+                ret = -ENOMEDIUM;
+                goto err;
+        }
+
+        /* now fetch the board file */
+        if (ar->hw_params.fw.board == NULL) {
+                ath10k_err("board data file not defined");
+                ret = -EINVAL;
+                goto err;
+        }
+
+        ar->board = ath10k_fetch_fw_file(ar,
+                                         ar->hw_params.fw.dir,
+                                         ar->hw_params.fw.board);
+        if (IS_ERR(ar->board)) {
+                ret = PTR_ERR(ar->board);
+                ath10k_err("could not fetch board data (%d)\n", ret);
+                goto err;
+        }
+
+        ar->board_data = ar->board->data;
+        ar->board_len = ar->board->size;
+
+        return 0;
+
+err:
+        ath10k_core_free_firmware_files(ar);
+        return ret;
+}
+
+static int ath10k_core_fetch_firmware_files(struct ath10k *ar)
+{
+        int ret;
+
+        ret = ath10k_core_fetch_firmware_api_n(ar, ATH10K_FW_API2_FILE);
+        if (ret == 0) {
+                ar->fw_api = 2;
+                goto out;
+        }
+
+        ret = ath10k_core_fetch_firmware_api_1(ar);
+        if (ret)
+                return ret;
+
+        ar->fw_api = 1;
+
+out:
+        ath10k_dbg(ATH10K_DBG_BOOT, "using fw api %d\n", ar->fw_api);
+
+        return 0;
+}
+
 static int ath10k_init_download_firmware(struct ath10k *ar)
 {
         int ret;
@@ -541,6 +720,9 @@ struct ath10k *ath10k_core_create(void *hif_priv, struct device *dev,
         INIT_WORK(&ar->offchan_tx_work, ath10k_offchan_tx_work);
         skb_queue_head_init(&ar->offchan_tx_queue);
 
+        INIT_WORK(&ar->wmi_mgmt_tx_work, ath10k_mgmt_over_wmi_tx_work);
+        skb_queue_head_init(&ar->wmi_mgmt_tx_queue);
+
         init_waitqueue_head(&ar->event_queue);
 
         INIT_WORK(&ar->restart_work, ath10k_core_restart);
@@ -555,6 +737,8 @@ EXPORT_SYMBOL(ath10k_core_create);
 
 void ath10k_core_destroy(struct ath10k *ar)
 {
+        ath10k_debug_destroy(ar);
+
         flush_workqueue(ar->workqueue);
         destroy_workqueue(ar->workqueue);
 
@@ -566,6 +750,8 @@ int ath10k_core_start(struct ath10k *ar)
 {
         int status;
 
+        lockdep_assert_held(&ar->conf_mutex);
+
         ath10k_bmi_start(ar);
 
         if (ath10k_init_configure_target(ar)) {
@@ -616,10 +802,6 @@ int ath10k_core_start(struct ath10k *ar)
 
         ath10k_info("firmware %s booted\n", ar->hw->wiphy->fw_version);
 
-        status = ath10k_check_fw_version(ar);
-        if (status)
-                goto err_disconnect_htc;
-
         status = ath10k_wmi_cmd_init(ar);
         if (status) {
                 ath10k_err("could not send WMI init command (%d)\n", status);
@@ -642,6 +824,7 @@ int ath10k_core_start(struct ath10k *ar)
                 goto err_disconnect_htc;
 
         ar->free_vdev_map = (1 << TARGET_NUM_VDEVS) - 1;
+        INIT_LIST_HEAD(&ar->arvifs);
 
         return 0;
 
@@ -658,6 +841,8 @@ EXPORT_SYMBOL(ath10k_core_start);
 
 void ath10k_core_stop(struct ath10k *ar)
 {
+        lockdep_assert_held(&ar->conf_mutex);
+
         ath10k_debug_stop(ar);
         ath10k_htc_stop(&ar->htc);
         ath10k_htt_detach(&ar->htt);
@@ -705,15 +890,21 @@ static int ath10k_core_probe_fw(struct ath10k *ar)
                 return ret;
         }
 
+        mutex_lock(&ar->conf_mutex);
+
         ret = ath10k_core_start(ar);
         if (ret) {
                 ath10k_err("could not init core (%d)\n", ret);
                 ath10k_core_free_firmware_files(ar);
                 ath10k_hif_power_down(ar);
+                mutex_unlock(&ar->conf_mutex);
                 return ret;
         }
 
         ath10k_core_stop(ar);
+
+        mutex_unlock(&ar->conf_mutex);
+
         ath10k_hif_power_down(ar);
         return 0;
 }
diff --git a/drivers/net/wireless/ath/ath10k/core.h b/drivers/net/wireless/ath/ath10k/core.h
index 292ad4577c98..0934f7633de3 100644
--- a/drivers/net/wireless/ath/ath10k/core.h
+++ b/drivers/net/wireless/ath/ath10k/core.h
@@ -43,15 +43,17 @@
 /* Antenna noise floor */
 #define ATH10K_DEFAULT_NOISE_FLOOR -95
 
+#define ATH10K_MAX_NUM_MGMT_PENDING 16
+
 struct ath10k;
 
 struct ath10k_skb_cb {
         dma_addr_t paddr;
         bool is_mapped;
         bool is_aborted;
+        u8 vdev_id;
 
         struct {
-                u8 vdev_id;
                 u8 tid;
                 bool is_offchan;
 
@@ -102,11 +104,26 @@ struct ath10k_bmi {
         bool done_sent;
 };
 
+#define ATH10K_MAX_MEM_REQS 16
+
+struct ath10k_mem_chunk {
+        void *vaddr;
+        dma_addr_t paddr;
+        u32 len;
+        u32 req_id;
+};
+
 struct ath10k_wmi {
         enum ath10k_htc_ep_id eid;
         struct completion service_ready;
         struct completion unified_ready;
         wait_queue_head_t tx_credits_wq;
+        struct wmi_cmd_map *cmd;
+        struct wmi_vdev_param_map *vdev_param;
+        struct wmi_pdev_param_map *pdev_param;
+
+        u32 num_mem_chunks;
+        struct ath10k_mem_chunk mem_chunks[ATH10K_MAX_MEM_REQS];
 };
 
 struct ath10k_peer_stat {
@@ -188,6 +205,8 @@ struct ath10k_peer {
 #define ATH10K_VDEV_SETUP_TIMEOUT_HZ (5*HZ)
 
 struct ath10k_vif {
+        struct list_head list;
+
         u32 vdev_id;
         enum wmi_vdev_type vdev_type;
         enum wmi_vdev_subtype vdev_subtype;
@@ -198,8 +217,10 @@ struct ath10k_vif {
         struct ath10k *ar;
         struct ieee80211_vif *vif;
 
+        struct work_struct wep_key_work;
         struct ieee80211_key_conf *wep_keys[WMI_MAX_KEY_INDEX + 1];
-        u8 def_wep_key_index;
+        u8 def_wep_key_idx;
+        u8 def_wep_key_newidx;
 
         u16 tx_seq_no;
 
@@ -268,6 +289,12 @@ enum ath10k_fw_features {
         /* wmi_mgmt_rx_hdr contains extra RSSI information */
         ATH10K_FW_FEATURE_EXT_WMI_MGMT_RX = 0,
 
+        /* firmware from 10X branch */
+        ATH10K_FW_FEATURE_WMI_10X = 1,
+
+        /* firmware support tx frame management over WMI, otherwise it's HTT */
+        ATH10K_FW_FEATURE_HAS_WMI_MGMT_TX = 2,
+
         /* keep last */
         ATH10K_FW_FEATURE_COUNT,
 };
@@ -324,9 +351,19 @@ struct ath10k {
                 } fw;
         } hw_params;
 
-        const struct firmware *board_data;
+        const struct firmware *board;
+        const void *board_data;
+        size_t board_len;
+
         const struct firmware *otp;
+        const void *otp_data;
+        size_t otp_len;
+
         const struct firmware *firmware;
+        const void *firmware_data;
+        size_t firmware_len;
+
+        int fw_api;
 
         struct {
                 struct completion started;
@@ -369,6 +406,7 @@ struct ath10k {
         /* protects shared structure data */
         spinlock_t data_lock;
 
+        struct list_head arvifs;
         struct list_head peers;
         wait_queue_head_t peer_mapping_wq;
 
@@ -377,6 +415,9 @@ struct ath10k {
         struct completion offchan_tx_completed;
         struct sk_buff *offchan_tx_skb;
 
+        struct work_struct wmi_mgmt_tx_work;
+        struct sk_buff_head wmi_mgmt_tx_queue;
+
         enum ath10k_state state;
 
         struct work_struct restart_work;
diff --git a/drivers/net/wireless/ath/ath10k/debug.c b/drivers/net/wireless/ath/ath10k/debug.c
index 59615c7f217e..760ff2289e3c 100644
--- a/drivers/net/wireless/ath/ath10k/debug.c
+++ b/drivers/net/wireless/ath/ath10k/debug.c
@@ -618,6 +618,8 @@ int ath10k_debug_start(struct ath10k *ar)
 {
         int ret;
 
+        lockdep_assert_held(&ar->conf_mutex);
+
         ret = ath10k_debug_htt_stats_req(ar);
         if (ret)
                 /* continue normally anyway, this isn't serious */
@@ -628,7 +630,13 @@ int ath10k_debug_start(struct ath10k *ar)
 
 void ath10k_debug_stop(struct ath10k *ar)
 {
-        cancel_delayed_work_sync(&ar->debug.htt_stats_dwork);
+        lockdep_assert_held(&ar->conf_mutex);
+
+        /* Must not use _sync to avoid deadlock, we do that in
+         * ath10k_debug_destroy(). The check for htt_stats_mask is to avoid
+         * warning from del_timer(). */
+        if (ar->debug.htt_stats_mask != 0)
+                cancel_delayed_work(&ar->debug.htt_stats_dwork);
 }
 
 int ath10k_debug_create(struct ath10k *ar)
@@ -662,6 +670,11 @@ int ath10k_debug_create(struct ath10k *ar)
         return 0;
 }
 
+void ath10k_debug_destroy(struct ath10k *ar)
+{
+        cancel_delayed_work_sync(&ar->debug.htt_stats_dwork);
+}
+
 #endif /* CONFIG_ATH10K_DEBUGFS */
 
 #ifdef CONFIG_ATH10K_DEBUG
diff --git a/drivers/net/wireless/ath/ath10k/debug.h b/drivers/net/wireless/ath/ath10k/debug.h
index 6576b82a8d86..3cfe3ee90dbe 100644
--- a/drivers/net/wireless/ath/ath10k/debug.h
+++ b/drivers/net/wireless/ath/ath10k/debug.h
@@ -46,6 +46,7 @@ __printf(1, 2) int ath10k_warn(const char *fmt, ...);
 int ath10k_debug_start(struct ath10k *ar);
 void ath10k_debug_stop(struct ath10k *ar);
 int ath10k_debug_create(struct ath10k *ar);
+void ath10k_debug_destroy(struct ath10k *ar);
 void ath10k_debug_read_service_map(struct ath10k *ar,
                                    void *service_map,
                                    size_t map_size);
@@ -67,6 +68,10 @@ static inline int ath10k_debug_create(struct ath10k *ar)
         return 0;
 }
 
+static inline void ath10k_debug_destroy(struct ath10k *ar)
+{
+}
+
 static inline void ath10k_debug_read_service_map(struct ath10k *ar,
                                                  void *service_map,
                                                  size_t map_size)
diff --git a/drivers/net/wireless/ath/ath10k/htt_tx.c b/drivers/net/wireless/ath/ath10k/htt_tx.c
index 3b93c6a01c6c..d9335e9d0d04 100644
--- a/drivers/net/wireless/ath/ath10k/htt_tx.c
+++ b/drivers/net/wireless/ath/ath10k/htt_tx.c
@@ -308,7 +308,7 @@ int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
         struct sk_buff *txdesc = NULL;
         struct htt_cmd *cmd;
         struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(msdu);
-        u8 vdev_id = skb_cb->htt.vdev_id;
+        u8 vdev_id = skb_cb->vdev_id;
         int len = 0;
         int msdu_id = -1;
         int res;
@@ -384,7 +384,7 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
         struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(msdu);
         struct sk_buff *txdesc = NULL;
         bool use_frags;
-        u8 vdev_id = ATH10K_SKB_CB(msdu)->htt.vdev_id;
+        u8 vdev_id = ATH10K_SKB_CB(msdu)->vdev_id;
         u8 tid;
         int prefetch_len, desc_len;
         int msdu_id = -1;
diff --git a/drivers/net/wireless/ath/ath10k/hw.h b/drivers/net/wireless/ath/ath10k/hw.h
index 8c1be7685922..8aeb46d9b534 100644
--- a/drivers/net/wireless/ath/ath10k/hw.h
+++ b/drivers/net/wireless/ath/ath10k/hw.h
@@ -20,12 +20,6 @@
 
 #include "targaddrs.h"
 
-/* Supported FW version */
-#define SUPPORTED_FW_MAJOR      1
-#define SUPPORTED_FW_MINOR      0
-#define SUPPORTED_FW_RELEASE    0
-#define SUPPORTED_FW_BUILD      636
-
 /* QCA988X 1.0 definitions (unsupported) */
 #define QCA988X_HW_1_0_CHIP_ID_REV      0x0
 
@@ -38,6 +32,25 @@
 #define QCA988X_HW_2_0_BOARD_DATA_FILE  "board.bin"
 #define QCA988X_HW_2_0_PATCH_LOAD_ADDR  0x1234
 
+#define ATH10K_FW_API2_FILE             "firmware-2.bin"
+
+/* includes also the null byte */
+#define ATH10K_FIRMWARE_MAGIC               "QCA-ATH10K"
+
+struct ath10k_fw_ie {
+        __le32 id;
+        __le32 len;
+        u8 data[0];
+};
+
+enum ath10k_fw_ie_type {
+        ATH10K_FW_IE_FW_VERSION = 0,
+        ATH10K_FW_IE_TIMESTAMP = 1,
+        ATH10K_FW_IE_FEATURES = 2,
+        ATH10K_FW_IE_FW_IMAGE = 3,
+        ATH10K_FW_IE_OTP_IMAGE = 4,
+};
+
 /* Known pecularities:
  *  - current FW doesn't support raw rx mode (last tested v599)
  *  - current FW dumps upon raw tx mode (last tested v599)
@@ -59,6 +72,7 @@ enum ath10k_mcast2ucast_mode {
         ATH10K_MCAST2UCAST_ENABLED = 1,
 };
 
+/* Target specific defines for MAIN firmware */
 #define TARGET_NUM_VDEVS                        8
 #define TARGET_NUM_PEER_AST                     2
 #define TARGET_NUM_WDS_ENTRIES                  32
@@ -93,6 +107,36 @@ enum ath10k_mcast2ucast_mode {
 #define TARGET_NUM_MSDU_DESC                    (1024 + 400)
 #define TARGET_MAX_FRAG_ENTRIES                 0
 
+/* Target specific defines for 10.X firmware */
+#define TARGET_10X_NUM_VDEVS                    16
+#define TARGET_10X_NUM_PEER_AST                 2
+#define TARGET_10X_NUM_WDS_ENTRIES              32
+#define TARGET_10X_DMA_BURST_SIZE               0
+#define TARGET_10X_MAC_AGGR_DELIM               0
+#define TARGET_10X_AST_SKID_LIMIT               16
+#define TARGET_10X_NUM_PEERS                    (128 + (TARGET_10X_NUM_VDEVS))
+#define TARGET_10X_NUM_OFFLOAD_PEERS            0
+#define TARGET_10X_NUM_OFFLOAD_REORDER_BUFS     0
+#define TARGET_10X_NUM_PEER_KEYS                2
+#define TARGET_10X_NUM_TIDS                     256
+#define TARGET_10X_TX_CHAIN_MASK                (BIT(0) | BIT(1) | BIT(2))
+#define TARGET_10X_RX_CHAIN_MASK                (BIT(0) | BIT(1) | BIT(2))
+#define TARGET_10X_RX_TIMEOUT_LO_PRI            100
+#define TARGET_10X_RX_TIMEOUT_HI_PRI            40
+#define TARGET_10X_RX_DECAP_MODE                ATH10K_HW_TXRX_NATIVE_WIFI
+#define TARGET_10X_SCAN_MAX_PENDING_REQS        4
+#define TARGET_10X_BMISS_OFFLOAD_MAX_VDEV       2
+#define TARGET_10X_ROAM_OFFLOAD_MAX_VDEV        2
+#define TARGET_10X_ROAM_OFFLOAD_MAX_AP_PROFILES 8
+#define TARGET_10X_GTK_OFFLOAD_MAX_VDEV         3
+#define TARGET_10X_NUM_MCAST_GROUPS             0
+#define TARGET_10X_NUM_MCAST_TABLE_ELEMS        0
+#define TARGET_10X_MCAST2UCAST_MODE             ATH10K_MCAST2UCAST_DISABLED
+#define TARGET_10X_TX_DBG_LOG_SIZE              1024
+#define TARGET_10X_RX_SKIP_DEFRAG_TIMEOUT_DUP_DETECTION_CHECK 1
+#define TARGET_10X_VOW_CONFIG                   0
+#define TARGET_10X_NUM_MSDU_DESC                (1024 + 400)
+#define TARGET_10X_MAX_FRAG_ENTRIES             0
 
 /* Number of Copy Engines supported */
 #define CE_COUNT 8
diff --git a/drivers/net/wireless/ath/ath10k/mac.c b/drivers/net/wireless/ath/ath10k/mac.c
index 99a9bad3f398..0b1cc516e778 100644
--- a/drivers/net/wireless/ath/ath10k/mac.c
+++ b/drivers/net/wireless/ath/ath10k/mac.c
@@ -334,25 +334,29 @@ static int ath10k_peer_create(struct ath10k *ar, u32 vdev_id, const u8 *addr)
 
 static int  ath10k_mac_set_rts(struct ath10k_vif *arvif, u32 value)
 {
+        struct ath10k *ar = arvif->ar;
+        u32 vdev_param;
+
         if (value != 0xFFFFFFFF)
                 value = min_t(u32, arvif->ar->hw->wiphy->rts_threshold,
                               ATH10K_RTS_MAX);
 
-        return ath10k_wmi_vdev_set_param(arvif->ar, arvif->vdev_id,
-                                         WMI_VDEV_PARAM_RTS_THRESHOLD,
-                                         value);
+        vdev_param = ar->wmi.vdev_param->rts_threshold;
+        return ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param, value);
 }
 
 static int ath10k_mac_set_frag(struct ath10k_vif *arvif, u32 value)
 {
+        struct ath10k *ar = arvif->ar;
+        u32 vdev_param;
+
         if (value != 0xFFFFFFFF)
                 value = clamp_t(u32, arvif->ar->hw->wiphy->frag_threshold,
                                 ATH10K_FRAGMT_THRESHOLD_MIN,
                                 ATH10K_FRAGMT_THRESHOLD_MAX);
 
-        return ath10k_wmi_vdev_set_param(arvif->ar, arvif->vdev_id,
-                                         WMI_VDEV_PARAM_FRAGMENTATION_THRESHOLD,
-                                         value);
+        vdev_param = ar->wmi.vdev_param->fragmentation_threshold;
+        return ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param, value);
 }
 
 static int ath10k_peer_delete(struct ath10k *ar, u32 vdev_id, const u8 *addr)
@@ -562,12 +566,9 @@ static int ath10k_monitor_stop(struct ath10k *ar)
 
         lockdep_assert_held(&ar->conf_mutex);
 
-        /* For some reasons, ath10k_wmi_vdev_down() here couse
-         * often ath10k_wmi_vdev_stop() to fail. Next we could
-         * not run monitor vdev and driver reload
-         * required. Don't see such problems we skip
-         * ath10k_wmi_vdev_down() here.
-         */
+        ret = ath10k_wmi_vdev_down(ar, ar->monitor_vdev_id);
+        if (ret)
+                ath10k_warn("Monitor vdev down failed: %d\n", ret);
 
         ret = ath10k_wmi_vdev_stop(ar, ar->monitor_vdev_id);
         if (ret)
@@ -677,6 +678,7 @@ static void ath10k_control_ibss(struct ath10k_vif *arvif,
                                 struct ieee80211_bss_conf *info,
                                 const u8 self_peer[ETH_ALEN])
 {
+        u32 vdev_param;
         int ret = 0;
 
         lockdep_assert_held(&arvif->ar->conf_mutex);
@@ -710,8 +712,8 @@ static void ath10k_control_ibss(struct ath10k_vif *arvif,
                 return;
         }
 
-        ret = ath10k_wmi_vdev_set_param(arvif->ar, arvif->vdev_id,
-                                        WMI_VDEV_PARAM_ATIM_WINDOW,
+        vdev_param = arvif->ar->wmi.vdev_param->atim_window;
+        ret = ath10k_wmi_vdev_set_param(arvif->ar, arvif->vdev_id, vdev_param,
                                         ATH10K_DEFAULT_ATIM);
         if (ret)
                 ath10k_warn("Failed to set IBSS ATIM for VDEV:%d ret:%d\n",
@@ -721,35 +723,30 @@ static void ath10k_control_ibss(struct ath10k_vif *arvif,
 /*
  * Review this when mac80211 gains per-interface powersave support.
  */
-static void ath10k_ps_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
+static int ath10k_mac_vif_setup_ps(struct ath10k_vif *arvif)
 {
-        struct ath10k_generic_iter *ar_iter = data;
-        struct ieee80211_conf *conf = &ar_iter->ar->hw->conf;
-        struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+        struct ath10k *ar = arvif->ar;
+        struct ieee80211_conf *conf = &ar->hw->conf;
         enum wmi_sta_powersave_param param;
         enum wmi_sta_ps_mode psmode;
         int ret;
 
         lockdep_assert_held(&arvif->ar->conf_mutex);
 
-        if (vif->type != NL80211_IFTYPE_STATION)
-                return;
+        if (arvif->vif->type != NL80211_IFTYPE_STATION)
+                return 0;
 
         if (conf->flags & IEEE80211_CONF_PS) {
                 psmode = WMI_STA_PS_MODE_ENABLED;
                 param = WMI_STA_PS_PARAM_INACTIVITY_TIME;
 
-                ret = ath10k_wmi_set_sta_ps_param(ar_iter->ar,
-                                                  arvif->vdev_id,
-                                                  param,
+                ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id, param,
                                                   conf->dynamic_ps_timeout);
                 if (ret) {
                         ath10k_warn("Failed to set inactivity time for VDEV: %d\n",
                                     arvif->vdev_id);
-                        return;
+                        return ret;
                 }
-
-                ar_iter->ret = ret;
         } else {
                 psmode = WMI_STA_PS_MODE_DISABLED;
         }
@@ -757,11 +754,14 @@ static void ath10k_ps_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
         ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d psmode %s\n",
                    arvif->vdev_id, psmode ? "enable" : "disable");
 
-        ar_iter->ret = ath10k_wmi_set_psmode(ar_iter->ar, arvif->vdev_id,
-                                             psmode);
-        if (ar_iter->ret)
+        ret = ath10k_wmi_set_psmode(ar, arvif->vdev_id, psmode);
+        if (ret) {
                 ath10k_warn("Failed to set PS Mode: %d for VDEV: %d\n",
                             psmode, arvif->vdev_id);
+                return ret;
+        }
+
+        return 0;
 }
 
 /**********************/
@@ -1031,14 +1031,27 @@ static void ath10k_peer_assoc_h_vht(struct ath10k *ar,
                                     struct wmi_peer_assoc_complete_arg *arg)
 {
         const struct ieee80211_sta_vht_cap *vht_cap = &sta->vht_cap;
+        u8 ampdu_factor;
 
         if (!vht_cap->vht_supported)
                 return;
 
         arg->peer_flags |= WMI_PEER_VHT;
-
         arg->peer_vht_caps = vht_cap->cap;
 
+
+        ampdu_factor = (vht_cap->cap &
+                        IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK) >>
+                       IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_SHIFT;
+
+        /* Workaround: Some Netgear/Linksys 11ac APs set Rx A-MPDU factor to
+         * zero in VHT IE. Using it would result in degraded throughput.
+         * arg->peer_max_mpdu at this point contains HT max_mpdu so keep
+         * it if VHT max_mpdu is smaller. */
+        arg->peer_max_mpdu = max(arg->peer_max_mpdu,
+                                 (1U << (IEEE80211_HT_MAX_AMPDU_FACTOR +
+                                        ampdu_factor)) - 1);
+
         if (sta->bandwidth == IEEE80211_STA_RX_BW_80)
                 arg->peer_flags |= WMI_PEER_80MHZ;
 
@@ -1124,26 +1137,25 @@ static void ath10k_peer_assoc_h_phymode(struct ath10k *ar,
         WARN_ON(phymode == MODE_UNKNOWN);
 }
 
-static int ath10k_peer_assoc(struct ath10k *ar,
-                             struct ath10k_vif *arvif,
-                             struct ieee80211_sta *sta,
-                             struct ieee80211_bss_conf *bss_conf)
+static int ath10k_peer_assoc_prepare(struct ath10k *ar,
+                                     struct ath10k_vif *arvif,
+                                     struct ieee80211_sta *sta,
+                                     struct ieee80211_bss_conf *bss_conf,
+                                     struct wmi_peer_assoc_complete_arg *arg)
 {
-        struct wmi_peer_assoc_complete_arg arg;
-
         lockdep_assert_held(&ar->conf_mutex);
 
-        memset(&arg, 0, sizeof(struct wmi_peer_assoc_complete_arg));
+        memset(arg, 0, sizeof(*arg));
 
-        ath10k_peer_assoc_h_basic(ar, arvif, sta, bss_conf, &arg);
-        ath10k_peer_assoc_h_crypto(ar, arvif, &arg);
-        ath10k_peer_assoc_h_rates(ar, sta, &arg);
-        ath10k_peer_assoc_h_ht(ar, sta, &arg);
-        ath10k_peer_assoc_h_vht(ar, sta, &arg);
-        ath10k_peer_assoc_h_qos(ar, arvif, sta, bss_conf, &arg);
-        ath10k_peer_assoc_h_phymode(ar, arvif, sta, &arg);
+        ath10k_peer_assoc_h_basic(ar, arvif, sta, bss_conf, arg);
+        ath10k_peer_assoc_h_crypto(ar, arvif, arg);
+        ath10k_peer_assoc_h_rates(ar, sta, arg);
+        ath10k_peer_assoc_h_ht(ar, sta, arg);
+        ath10k_peer_assoc_h_vht(ar, sta, arg);
+        ath10k_peer_assoc_h_qos(ar, arvif, sta, bss_conf, arg);
+        ath10k_peer_assoc_h_phymode(ar, arvif, sta, arg);
 
-        return ath10k_wmi_peer_assoc(ar, &arg);
+        return 0;
 }
 
 /* can be called only in mac80211 callbacks due to `key_count` usage */
@@ -1153,6 +1165,7 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 {
         struct ath10k *ar = hw->priv;
         struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
+        struct wmi_peer_assoc_complete_arg peer_arg;
         struct ieee80211_sta *ap_sta;
         int ret;
 
@@ -1168,15 +1181,24 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
                 return;
         }
 
-        ret = ath10k_peer_assoc(ar, arvif, ap_sta, bss_conf);
+        ret = ath10k_peer_assoc_prepare(ar, arvif, ap_sta,
+                                        bss_conf, &peer_arg);
         if (ret) {
-                ath10k_warn("Peer assoc failed for %pM\n", bss_conf->bssid);
+                ath10k_warn("Peer assoc prepare failed for %pM\n: %d",
+                            bss_conf->bssid, ret);
                 rcu_read_unlock();
                 return;
         }
 
         rcu_read_unlock();
 
+        ret = ath10k_wmi_peer_assoc(ar, &peer_arg);
+        if (ret) {
+                ath10k_warn("Peer assoc failed for %pM\n: %d",
+                            bss_conf->bssid, ret);
+                return;
+        }
+
         ath10k_dbg(ATH10K_DBG_MAC,
                    "mac vdev %d up (associated) bssid %pM aid %d\n",
                    arvif->vdev_id, bss_conf->bssid, bss_conf->aid);
@@ -1224,19 +1246,28 @@ static void ath10k_bss_disassoc(struct ieee80211_hw *hw,
         /* FIXME: why don't we print error if wmi call fails? */
         ret = ath10k_wmi_vdev_down(ar, arvif->vdev_id);
 
-        arvif->def_wep_key_index = 0;
+        arvif->def_wep_key_idx = 0;
 }
 
 static int ath10k_station_assoc(struct ath10k *ar, struct ath10k_vif *arvif,
                                 struct ieee80211_sta *sta)
 {
+        struct wmi_peer_assoc_complete_arg peer_arg;
         int ret = 0;
 
         lockdep_assert_held(&ar->conf_mutex);
 
-        ret = ath10k_peer_assoc(ar, arvif, sta, NULL);
+        ret = ath10k_peer_assoc_prepare(ar, arvif, sta, NULL, &peer_arg);
         if (ret) {
-                ath10k_warn("WMI peer assoc failed for %pM\n", sta->addr);
+                ath10k_warn("WMI peer assoc prepare failed for %pM\n",
+                            sta->addr);
+                return ret;
+        }
+
+        ret = ath10k_wmi_peer_assoc(ar, &peer_arg);
+        if (ret) {
+                ath10k_warn("Peer assoc failed for STA %pM\n: %d",
+                            sta->addr, ret);
                 return ret;
         }
 
@@ -1405,6 +1436,33 @@ static void ath10k_reg_notifier(struct wiphy *wiphy,
 /* TX handlers */
 /***************/
 
+static u8 ath10k_tx_h_get_tid(struct ieee80211_hdr *hdr)
+{
+        if (ieee80211_is_mgmt(hdr->frame_control))
+                return HTT_DATA_TX_EXT_TID_MGMT;
+
+        if (!ieee80211_is_data_qos(hdr->frame_control))
+                return HTT_DATA_TX_EXT_TID_NON_QOS_MCAST_BCAST;
+
+        if (!is_unicast_ether_addr(ieee80211_get_DA(hdr)))
+                return HTT_DATA_TX_EXT_TID_NON_QOS_MCAST_BCAST;
+
+        return ieee80211_get_qos_ctl(hdr)[0] & IEEE80211_QOS_CTL_TID_MASK;
+}
+
+static u8 ath10k_tx_h_get_vdev_id(struct ath10k *ar,
+                                  struct ieee80211_tx_info *info)
+{
+        if (info->control.vif)
+                return ath10k_vif_to_arvif(info->control.vif)->vdev_id;
+
+        if (ar->monitor_enabled)
+                return ar->monitor_vdev_id;
+
+        ath10k_warn("could not resolve vdev id\n");
+        return 0;
+}
+
 /*
  * Frames sent to the FW have to be in "Native Wifi" format.
  * Strip the QoS field from the 802.11 header.
@@ -1425,6 +1483,30 @@ static void ath10k_tx_h_qos_workaround(struct ieee80211_hw *hw,
         skb_pull(skb, IEEE80211_QOS_CTL_LEN);
 }
 
+static void ath10k_tx_wep_key_work(struct work_struct *work)
+{
+        struct ath10k_vif *arvif = container_of(work, struct ath10k_vif,
+                                                wep_key_work);
+        int ret, keyidx = arvif->def_wep_key_newidx;
+
+        if (arvif->def_wep_key_idx == keyidx)
+                return;
+
+        ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d set keyidx %d\n",
+                   arvif->vdev_id, keyidx);
+
+        ret = ath10k_wmi_vdev_set_param(arvif->ar,
+                                        arvif->vdev_id,
+                                        arvif->ar->wmi.vdev_param->def_keyid,
+                                        keyidx);
+        if (ret) {
+                ath10k_warn("could not update wep keyidx (%d)\n", ret);
+                return;
+        }
+
+        arvif->def_wep_key_idx = keyidx;
+}
+
 static void ath10k_tx_h_update_wep_key(struct sk_buff *skb)
 {
         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
@@ -1433,7 +1515,6 @@ static void ath10k_tx_h_update_wep_key(struct sk_buff *skb)
         struct ath10k *ar = arvif->ar;
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
         struct ieee80211_key_conf *key = info->control.hw_key;
-        int ret;
 
         if (!ieee80211_has_protected(hdr->frame_control))
                 return;
@@ -1445,21 +1526,14 @@ static void ath10k_tx_h_update_wep_key(struct sk_buff *skb)
             key->cipher != WLAN_CIPHER_SUITE_WEP104)
                 return;
 
-        if (key->keyidx == arvif->def_wep_key_index)
-                return;
-
-        ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d keyidx %d\n",
-                   arvif->vdev_id, key->keyidx);
-
-        ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id,
-                                        WMI_VDEV_PARAM_DEF_KEYID,
-                                        key->keyidx);
-        if (ret) {
-                ath10k_warn("could not update wep keyidx (%d)\n", ret);
+        if (key->keyidx == arvif->def_wep_key_idx)
                 return;
-        }
 
-        arvif->def_wep_key_index = key->keyidx;
+        /* FIXME: Most likely a few frames will be TXed with an old key. Simply
+         * queueing frames until key index is updated is not an option because
+         * sk_buff may need more processing to be done, e.g. offchannel */
+        arvif->def_wep_key_newidx = key->keyidx;
+        ieee80211_queue_work(ar->hw, &arvif->wep_key_work);
 }
 
 static void ath10k_tx_h_add_p2p_noa_ie(struct ath10k *ar, struct sk_buff *skb)
@@ -1489,7 +1563,7 @@ static void ath10k_tx_h_add_p2p_noa_ie(struct ath10k *ar, struct sk_buff *skb)
 static void ath10k_tx_htt(struct ath10k *ar, struct sk_buff *skb)
 {
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-        int ret;
+        int ret = 0;
 
         if (ar->htt.target_version_major >= 3) {
                 /* Since HTT 3.0 there is no separate mgmt tx command */
@@ -1497,16 +1571,32 @@ static void ath10k_tx_htt(struct ath10k *ar, struct sk_buff *skb)
                 goto exit;
         }
 
-        if (ieee80211_is_mgmt(hdr->frame_control))
-                ret = ath10k_htt_mgmt_tx(&ar->htt, skb);
-        else if (ieee80211_is_nullfunc(hdr->frame_control))
+        if (ieee80211_is_mgmt(hdr->frame_control)) {
+                if (test_bit(ATH10K_FW_FEATURE_HAS_WMI_MGMT_TX,
+                             ar->fw_features)) {
+                        if (skb_queue_len(&ar->wmi_mgmt_tx_queue) >=
+                            ATH10K_MAX_NUM_MGMT_PENDING) {
+                                ath10k_warn("wmi mgmt_tx queue limit reached\n");
+                                ret = -EBUSY;
+                                goto exit;
+                        }
+
+                        skb_queue_tail(&ar->wmi_mgmt_tx_queue, skb);
+                        ieee80211_queue_work(ar->hw, &ar->wmi_mgmt_tx_work);
+                } else {
+                        ret = ath10k_htt_mgmt_tx(&ar->htt, skb);
+                }
+        } else if (!test_bit(ATH10K_FW_FEATURE_HAS_WMI_MGMT_TX,
+                             ar->fw_features) &&
+                   ieee80211_is_nullfunc(hdr->frame_control)) {
                 /* FW does not report tx status properly for NullFunc frames
                  * unless they are sent through mgmt tx path. mac80211 sends
-                 * those frames when it detects link/beacon loss and depends on
-                 * the tx status to be correct. */
+                 * those frames when it detects link/beacon loss and depends
+                 * on the tx status to be correct. */
                 ret = ath10k_htt_mgmt_tx(&ar->htt, skb);
-        else
+        } else {
                 ret = ath10k_htt_tx(&ar->htt, skb);
+        }
 
 exit:
         if (ret) {
@@ -1557,7 +1647,7 @@ void ath10k_offchan_tx_work(struct work_struct *work)
 
                 hdr = (struct ieee80211_hdr *)skb->data;
                 peer_addr = ieee80211_get_DA(hdr);
-                vdev_id = ATH10K_SKB_CB(skb)->htt.vdev_id;
+                vdev_id = ATH10K_SKB_CB(skb)->vdev_id;
 
                 spin_lock_bh(&ar->data_lock);
                 peer = ath10k_peer_find(ar, vdev_id, peer_addr);
@@ -1599,6 +1689,36 @@ void ath10k_offchan_tx_work(struct work_struct *work)
         }
 }
 
+void ath10k_mgmt_over_wmi_tx_purge(struct ath10k *ar)
+{
+        struct sk_buff *skb;
+
+        for (;;) {
+                skb = skb_dequeue(&ar->wmi_mgmt_tx_queue);
+                if (!skb)
+                        break;
+
+                ieee80211_free_txskb(ar->hw, skb);
+        }
+}
+
+void ath10k_mgmt_over_wmi_tx_work(struct work_struct *work)
+{
+        struct ath10k *ar = container_of(work, struct ath10k, wmi_mgmt_tx_work);
+        struct sk_buff *skb;
+        int ret;
+
+        for (;;) {
+                skb = skb_dequeue(&ar->wmi_mgmt_tx_queue);
+                if (!skb)
+                        break;
+
+                ret = ath10k_wmi_mgmt_tx(ar, skb);
+                if (ret)
+                        ath10k_warn("wmi mgmt_tx failed (%d)\n", ret);
+        }
+}
+
 /************/
 /* Scanning */
 /************/
@@ -1722,16 +1842,7 @@ static void ath10k_tx(struct ieee80211_hw *hw,
         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
         struct ath10k *ar = hw->priv;
-        struct ath10k_vif *arvif = NULL;
-        u32 vdev_id = 0;
-        u8 tid;
-
-        if (info->control.vif) {
-                arvif = ath10k_vif_to_arvif(info->control.vif);
-                vdev_id = arvif->vdev_id;
-        } else if (ar->monitor_enabled) {
-                vdev_id = ar->monitor_vdev_id;
-        }
+        u8 tid, vdev_id;
 
         /* We should disable CCK RATE due to P2P */
         if (info->flags & IEEE80211_TX_CTL_NO_CCK_RATE)
@@ -1739,14 +1850,8 @@ static void ath10k_tx(struct ieee80211_hw *hw,
 
         /* we must calculate tid before we apply qos workaround
          * as we'd lose the qos control field */
-        tid = HTT_DATA_TX_EXT_TID_NON_QOS_MCAST_BCAST;
-        if (ieee80211_is_mgmt(hdr->frame_control)) {
-                tid = HTT_DATA_TX_EXT_TID_MGMT;
-        } else if (ieee80211_is_data_qos(hdr->frame_control) &&
-                   is_unicast_ether_addr(ieee80211_get_DA(hdr))) {
-                u8 *qc = ieee80211_get_qos_ctl(hdr);
-                tid = qc[0] & IEEE80211_QOS_CTL_TID_MASK;
-        }
+        tid = ath10k_tx_h_get_tid(hdr);
+        vdev_id = ath10k_tx_h_get_vdev_id(ar, info);
 
         /* it makes no sense to process injected frames like that */
         if (info->control.vif &&
@@ -1757,14 +1862,14 @@ static void ath10k_tx(struct ieee80211_hw *hw,
                 ath10k_tx_h_seq_no(skb);
         }
 
+        ATH10K_SKB_CB(skb)->vdev_id = vdev_id;
         ATH10K_SKB_CB(skb)->htt.is_offchan = false;
-        ATH10K_SKB_CB(skb)->htt.vdev_id = vdev_id;
         ATH10K_SKB_CB(skb)->htt.tid = tid;
 
         if (info->flags & IEEE80211_TX_CTL_TX_OFFCHAN) {
                 spin_lock_bh(&ar->data_lock);
                 ATH10K_SKB_CB(skb)->htt.is_offchan = true;
-                ATH10K_SKB_CB(skb)->htt.vdev_id = ar->scan.vdev_id;
+                ATH10K_SKB_CB(skb)->vdev_id = ar->scan.vdev_id;
                 spin_unlock_bh(&ar->data_lock);
 
                 ath10k_dbg(ATH10K_DBG_MAC, "queued offchannel skb %p\n", skb);
@@ -1786,6 +1891,7 @@ void ath10k_halt(struct ath10k *ar)
 
         del_timer_sync(&ar->scan.timeout);
         ath10k_offchan_tx_purge(ar);
+        ath10k_mgmt_over_wmi_tx_purge(ar);
         ath10k_peer_cleanup_all(ar);
         ath10k_core_stop(ar);
         ath10k_hif_power_down(ar);
@@ -1832,12 +1938,12 @@ static int ath10k_start(struct ieee80211_hw *hw)
         else if (ar->state == ATH10K_STATE_RESTARTING)
                 ar->state = ATH10K_STATE_RESTARTED;
 
-        ret = ath10k_wmi_pdev_set_param(ar, WMI_PDEV_PARAM_PMF_QOS, 1);
+        ret = ath10k_wmi_pdev_set_param(ar, ar->wmi.pdev_param->pmf_qos, 1);
         if (ret)
                 ath10k_warn("could not enable WMI_PDEV_PARAM_PMF_QOS (%d)\n",
                             ret);
 
-        ret = ath10k_wmi_pdev_set_param(ar, WMI_PDEV_PARAM_DYNAMIC_BW, 0);
+        ret = ath10k_wmi_pdev_set_param(ar, ar->wmi.pdev_param->dynamic_bw, 0);
         if (ret)
                 ath10k_warn("could not init WMI_PDEV_PARAM_DYNAMIC_BW (%d)\n",
                             ret);
@@ -1862,32 +1968,29 @@ static void ath10k_stop(struct ieee80211_hw *hw)
         ar->state = ATH10K_STATE_OFF;
         mutex_unlock(&ar->conf_mutex);
 
+        ath10k_mgmt_over_wmi_tx_purge(ar);
+
         cancel_work_sync(&ar->offchan_tx_work);
+        cancel_work_sync(&ar->wmi_mgmt_tx_work);
         cancel_work_sync(&ar->restart_work);
 }
 
-static void ath10k_config_ps(struct ath10k *ar)
+static int ath10k_config_ps(struct ath10k *ar)
 {
-        struct ath10k_generic_iter ar_iter;
+        struct ath10k_vif *arvif;
+        int ret = 0;
 
         lockdep_assert_held(&ar->conf_mutex);
 
-        /* During HW reconfiguration mac80211 reports all interfaces that were
-         * running until reconfiguration was started. Since FW doesn't have any
-         * vdevs at this point we must not iterate over this interface list.
-         * This setting will be updated upon add_interface(). */
-        if (ar->state == ATH10K_STATE_RESTARTED)
-                return;
-
-        memset(&ar_iter, 0, sizeof(struct ath10k_generic_iter));
-        ar_iter.ar = ar;
-
-        ieee80211_iterate_active_interfaces_atomic(
-                ar->hw, IEEE80211_IFACE_ITER_NORMAL,
-                ath10k_ps_iter, &ar_iter);
+        list_for_each_entry(arvif, &ar->arvifs, list) {
+                ret = ath10k_mac_vif_setup_ps(arvif);
+                if (ret) {
+                        ath10k_warn("could not setup powersave (%d)\n", ret);
+                        break;
+                }
+        }
 
-        if (ar_iter.ret)
-                ath10k_warn("failed to set ps config (%d)\n", ar_iter.ret);
+        return ret;
 }
 
 static int ath10k_config(struct ieee80211_hw *hw, u32 changed)
@@ -1936,6 +2039,7 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
         int ret = 0;
         u32 value;
         int bit;
+        u32 vdev_param;
 
         mutex_lock(&ar->conf_mutex);
 
@@ -1944,21 +2048,22 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
         arvif->ar = ar;
         arvif->vif = vif;
 
+        INIT_WORK(&arvif->wep_key_work, ath10k_tx_wep_key_work);
+
         if ((vif->type == NL80211_IFTYPE_MONITOR) && ar->monitor_present) {
                 ath10k_warn("Only one monitor interface allowed\n");
                 ret = -EBUSY;
-                goto exit;
+                goto err;
         }
 
         bit = ffs(ar->free_vdev_map);
         if (bit == 0) {
                 ret = -EBUSY;
-                goto exit;
+                goto err;
         }
 
         arvif->vdev_id = bit - 1;
         arvif->vdev_subtype = WMI_VDEV_SUBTYPE_NONE;
-        ar->free_vdev_map &= ~(1 << arvif->vdev_id);
 
         if (ar->p2p)
                 arvif->vdev_subtype = WMI_VDEV_SUBTYPE_P2P_DEVICE;
@@ -1994,25 +2099,34 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
                                      arvif->vdev_subtype, vif->addr);
         if (ret) {
                 ath10k_warn("WMI vdev create failed: ret %d\n", ret);
-                goto exit;
+                goto err;
         }
 
-        ret = ath10k_wmi_vdev_set_param(ar, 0, WMI_VDEV_PARAM_DEF_KEYID,
-                                        arvif->def_wep_key_index);
-        if (ret)
+        ar->free_vdev_map &= ~BIT(arvif->vdev_id);
+        list_add(&arvif->list, &ar->arvifs);
+
+        vdev_param = ar->wmi.vdev_param->def_keyid;
+        ret = ath10k_wmi_vdev_set_param(ar, 0, vdev_param,
+                                        arvif->def_wep_key_idx);
+        if (ret) {
                 ath10k_warn("Failed to set default keyid: %d\n", ret);
+                goto err_vdev_delete;
+        }
 
-        ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id,
-                                        WMI_VDEV_PARAM_TX_ENCAP_TYPE,
+        vdev_param = ar->wmi.vdev_param->tx_encap_type;
+        ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
                                         ATH10K_HW_TXRX_NATIVE_WIFI);
-        if (ret)
+        /* 10.X firmware does not support this VDEV parameter. Do not warn */
+        if (ret && ret != -EOPNOTSUPP) {
                 ath10k_warn("Failed to set TX encap: %d\n", ret);
+                goto err_vdev_delete;
+        }
 
         if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
                 ret = ath10k_peer_create(ar, arvif->vdev_id, vif->addr);
                 if (ret) {
                         ath10k_warn("Failed to create peer for AP: %d\n", ret);
-                        goto exit;
+                        goto err_vdev_delete;
                 }
         }
 
@@ -2021,39 +2135,62 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
                 value = WMI_STA_PS_RX_WAKE_POLICY_WAKE;
                 ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id,
                                                   param, value);
-                if (ret)
+                if (ret) {
                         ath10k_warn("Failed to set RX wake policy: %d\n", ret);
+                        goto err_peer_delete;
+                }
 
                 param = WMI_STA_PS_PARAM_TX_WAKE_THRESHOLD;
                 value = WMI_STA_PS_TX_WAKE_THRESHOLD_ALWAYS;
                 ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id,
                                                   param, value);
-                if (ret)
+                if (ret) {
                         ath10k_warn("Failed to set TX wake thresh: %d\n", ret);
+                        goto err_peer_delete;
+                }
 
                 param = WMI_STA_PS_PARAM_PSPOLL_COUNT;
                 value = WMI_STA_PS_PSPOLL_COUNT_NO_MAX;
                 ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id,
                                                   param, value);
-                if (ret)
+                if (ret) {
                         ath10k_warn("Failed to set PSPOLL count: %d\n", ret);
+                        goto err_peer_delete;
+                }
         }
 
         ret = ath10k_mac_set_rts(arvif, ar->hw->wiphy->rts_threshold);
-        if (ret)
+        if (ret) {
                 ath10k_warn("failed to set rts threshold for vdev %d (%d)\n",
                             arvif->vdev_id, ret);
+                goto err_peer_delete;
+        }
 
         ret = ath10k_mac_set_frag(arvif, ar->hw->wiphy->frag_threshold);
-        if (ret)
+        if (ret) {
                 ath10k_warn("failed to set frag threshold for vdev %d (%d)\n",
                             arvif->vdev_id, ret);
+                goto err_peer_delete;
+        }
 
         if (arvif->vdev_type == WMI_VDEV_TYPE_MONITOR)
                 ar->monitor_present = true;
 
-exit:
         mutex_unlock(&ar->conf_mutex);
+        return 0;
+
+err_peer_delete:
+        if (arvif->vdev_type == WMI_VDEV_TYPE_AP)
+                ath10k_wmi_peer_delete(ar, arvif->vdev_id, vif->addr);
+
+err_vdev_delete:
+        ath10k_wmi_vdev_delete(ar, arvif->vdev_id);
+        ar->free_vdev_map &= ~BIT(arvif->vdev_id);
+        list_del(&arvif->list);
+
+err:
+        mutex_unlock(&ar->conf_mutex);
+
         return ret;
 }
 
@@ -2066,6 +2203,8 @@ static void ath10k_remove_interface(struct ieee80211_hw *hw,
 
         mutex_lock(&ar->conf_mutex);
 
+        cancel_work_sync(&arvif->wep_key_work);
+
         spin_lock_bh(&ar->data_lock);
         if (arvif->beacon) {
                 dev_kfree_skb_any(arvif->beacon);
@@ -2074,6 +2213,7 @@ static void ath10k_remove_interface(struct ieee80211_hw *hw,
         spin_unlock_bh(&ar->data_lock);
 
         ar->free_vdev_map |= 1 << (arvif->vdev_id);
+        list_del(&arvif->list);
 
         if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
                 ret = ath10k_peer_delete(arvif->ar, arvif->vdev_id, vif->addr);
@@ -2154,6 +2294,7 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
         struct ath10k *ar = hw->priv;
         struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
         int ret = 0;
+        u32 vdev_param, pdev_param;
 
         mutex_lock(&ar->conf_mutex);
 
@@ -2162,8 +2303,8 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 
         if (changed & BSS_CHANGED_BEACON_INT) {
                 arvif->beacon_interval = info->beacon_int;
-                ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id,
-                                                WMI_VDEV_PARAM_BEACON_INTERVAL,
+                vdev_param = ar->wmi.vdev_param->beacon_interval;
+                ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
                                                 arvif->beacon_interval);
                 ath10k_dbg(ATH10K_DBG_MAC,
                            "mac vdev %d beacon_interval %d\n",
@@ -2179,8 +2320,8 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
                            "vdev %d set beacon tx mode to staggered\n",
                            arvif->vdev_id);
 
-                ret = ath10k_wmi_pdev_set_param(ar,
-                                                WMI_PDEV_PARAM_BEACON_TX_MODE,
+                pdev_param = ar->wmi.pdev_param->beacon_tx_mode;
+                ret = ath10k_wmi_pdev_set_param(ar, pdev_param,
                                                 WMI_BEACON_STAGGERED_MODE);
                 if (ret)
                         ath10k_warn("Failed to set beacon mode for VDEV: %d\n",
@@ -2194,8 +2335,8 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
                            "mac vdev %d dtim_period %d\n",
                            arvif->vdev_id, arvif->dtim_period);
 
-                ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id,
-                                                WMI_VDEV_PARAM_DTIM_PERIOD,
+                vdev_param = ar->wmi.vdev_param->dtim_period;
+                ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
                                                 arvif->dtim_period);
                 if (ret)
                         ath10k_warn("Failed to set dtim period for VDEV: %d\n",
@@ -2262,8 +2403,8 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
                 ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d cts_prot %d\n",
                            arvif->vdev_id, cts_prot);
 
-                ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id,
-                                                WMI_VDEV_PARAM_ENABLE_RTSCTS,
+                vdev_param = ar->wmi.vdev_param->enable_rtscts;
+                ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
                                                 cts_prot);
                 if (ret)
                         ath10k_warn("Failed to set CTS prot for VDEV: %d\n",
@@ -2281,8 +2422,8 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
                 ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d slot_time %d\n",
                            arvif->vdev_id, slottime);
 
-                ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id,
-                                                WMI_VDEV_PARAM_SLOT_TIME,
+                vdev_param = ar->wmi.vdev_param->slot_time;
+                ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
                                                 slottime);
                 if (ret)
                         ath10k_warn("Failed to set erp slot for VDEV: %d\n",
@@ -2300,8 +2441,8 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
                            "mac vdev %d preamble %dn",
                            arvif->vdev_id, preamble);
 
-                ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id,
-                                                WMI_VDEV_PARAM_PREAMBLE,
+                vdev_param = ar->wmi.vdev_param->preamble;
+                ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
                                                 preamble);
                 if (ret)
                         ath10k_warn("Failed to set preamble for VDEV: %d\n",
@@ -2751,86 +2892,51 @@ static int ath10k_cancel_remain_on_channel(struct ieee80211_hw *hw)
  * Both RTS and Fragmentation threshold are interface-specific
  * in ath10k, but device-specific in mac80211.
  */
-static void ath10k_set_rts_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
-{
-        struct ath10k_generic_iter *ar_iter = data;
-        struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
-        u32 rts = ar_iter->ar->hw->wiphy->rts_threshold;
-
-        lockdep_assert_held(&arvif->ar->conf_mutex);
-
-        /* During HW reconfiguration mac80211 reports all interfaces that were
-         * running until reconfiguration was started. Since FW doesn't have any
-         * vdevs at this point we must not iterate over this interface list.
-         * This setting will be updated upon add_interface(). */
-        if (ar_iter->ar->state == ATH10K_STATE_RESTARTED)
-                return;
-
-        ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d rts_threshold %d\n",
-                   arvif->vdev_id, rts);
-
-        ar_iter->ret = ath10k_mac_set_rts(arvif, rts);
-        if (ar_iter->ret)
-                ath10k_warn("Failed to set RTS threshold for VDEV: %d\n",
-                            arvif->vdev_id);
-}
 
 static int ath10k_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
 {
-        struct ath10k_generic_iter ar_iter;
         struct ath10k *ar = hw->priv;
-
-        memset(&ar_iter, 0, sizeof(struct ath10k_generic_iter));
-        ar_iter.ar = ar;
+        struct ath10k_vif *arvif;
+        int ret = 0;
 
         mutex_lock(&ar->conf_mutex);
-        ieee80211_iterate_active_interfaces_atomic(
-                hw, IEEE80211_IFACE_ITER_NORMAL,
-                ath10k_set_rts_iter, &ar_iter);
-        mutex_unlock(&ar->conf_mutex);
-
-        return ar_iter.ret;
-}
-
-static void ath10k_set_frag_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
-{
-        struct ath10k_generic_iter *ar_iter = data;
-        struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
-        u32 frag = ar_iter->ar->hw->wiphy->frag_threshold;
-
-        lockdep_assert_held(&arvif->ar->conf_mutex);
+        list_for_each_entry(arvif, &ar->arvifs, list) {
+                ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d rts threshold %d\n",
+                           arvif->vdev_id, value);
 
-        /* During HW reconfiguration mac80211 reports all interfaces that were
-         * running until reconfiguration was started. Since FW doesn't have any
-         * vdevs at this point we must not iterate over this interface list.
-         * This setting will be updated upon add_interface(). */
-        if (ar_iter->ar->state == ATH10K_STATE_RESTARTED)
-                return;
-
-        ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d fragmentation_threshold %d\n",
-                   arvif->vdev_id, frag);
+                ret = ath10k_mac_set_rts(arvif, value);
+                if (ret) {
+                        ath10k_warn("could not set rts threshold for vdev %d (%d)\n",
+                                    arvif->vdev_id, ret);
+                        break;
+                }
+        }
+        mutex_unlock(&ar->conf_mutex);
 
-        ar_iter->ret = ath10k_mac_set_frag(arvif, frag);
-        if (ar_iter->ret)
-                ath10k_warn("Failed to set frag threshold for VDEV: %d\n",
-                            arvif->vdev_id);
+        return ret;
 }
 
 static int ath10k_set_frag_threshold(struct ieee80211_hw *hw, u32 value)
 {
-        struct ath10k_generic_iter ar_iter;
         struct ath10k *ar = hw->priv;
-
-        memset(&ar_iter, 0, sizeof(struct ath10k_generic_iter));
-        ar_iter.ar = ar;
+        struct ath10k_vif *arvif;
+        int ret = 0;
 
         mutex_lock(&ar->conf_mutex);
-        ieee80211_iterate_active_interfaces_atomic(
-                hw, IEEE80211_IFACE_ITER_NORMAL,
-                ath10k_set_frag_iter, &ar_iter);
+        list_for_each_entry(arvif, &ar->arvifs, list) {
+                ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d fragmentation threshold %d\n",
+                           arvif->vdev_id, value);
+
+                ret = ath10k_mac_set_rts(arvif, value);
+                if (ret) {
+                        ath10k_warn("could not set fragmentation threshold for vdev %d (%d)\n",
+                                    arvif->vdev_id, ret);
+                        break;
+                }
+        }
         mutex_unlock(&ar->conf_mutex);
 
-        return ar_iter.ret;
+        return ret;
 }
 
 static void ath10k_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
diff --git a/drivers/net/wireless/ath/ath10k/mac.h b/drivers/net/wireless/ath/ath10k/mac.h
index 6fce9bfb19a5..ba1021997b8f 100644
--- a/drivers/net/wireless/ath/ath10k/mac.h
+++ b/drivers/net/wireless/ath/ath10k/mac.h
@@ -34,6 +34,8 @@ struct ath10k_vif *ath10k_get_arvif(struct ath10k *ar, u32 vdev_id);
 void ath10k_reset_scan(unsigned long ptr);
 void ath10k_offchan_tx_purge(struct ath10k *ar);
 void ath10k_offchan_tx_work(struct work_struct *work);
+void ath10k_mgmt_over_wmi_tx_purge(struct ath10k *ar);
+void ath10k_mgmt_over_wmi_tx_work(struct work_struct *work);
 void ath10k_halt(struct ath10k *ar);
 
 static inline struct ath10k_vif *ath10k_vif_to_arvif(struct ieee80211_vif *vif)
diff --git a/drivers/net/wireless/ath/ath10k/pci.c b/drivers/net/wireless/ath/ath10k/pci.c
index dff23d97bed0..f8d59c7b9082 100644
--- a/drivers/net/wireless/ath/ath10k/pci.c
+++ b/drivers/net/wireless/ath/ath10k/pci.c
@@ -720,18 +720,8 @@ static int ath10k_pci_hif_send_head(struct ath10k *ar, u8 pipe_id,
                         "ath10k tx: data: ",
                         nbuf->data, nbuf->len);
 
-        /* Make sure we have resources to handle this request */
-        spin_lock_bh(&pipe_info->pipe_lock);
-        if (!pipe_info->num_sends_allowed) {
-                ath10k_warn("Pipe: %d is full\n", pipe_id);
-                spin_unlock_bh(&pipe_info->pipe_lock);
-                return -ENOSR;
-        }
-        pipe_info->num_sends_allowed--;
-        spin_unlock_bh(&pipe_info->pipe_lock);
-
-        ret = ath10k_ce_sendlist_send(ce_hdl, nbuf, transfer_id,
-                                      skb_cb->paddr, len, flags);
+        ret = ath10k_ce_send(ce_hdl, nbuf, skb_cb->paddr, len, transfer_id,
+                             flags);
         if (ret)
                 ath10k_warn("CE send failed: %p\n", nbuf);
 
@@ -741,14 +731,7 @@ static int ath10k_pci_hif_send_head(struct ath10k *ar, u8 pipe_id,
 static u16 ath10k_pci_hif_get_free_queue_number(struct ath10k *ar, u8 pipe)
 {
         struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-        struct ath10k_pci_pipe *pipe_info = &(ar_pci->pipe_info[pipe]);
-        int ret;
-
-        spin_lock_bh(&pipe_info->pipe_lock);
-        ret = pipe_info->num_sends_allowed;
-        spin_unlock_bh(&pipe_info->pipe_lock);
-
-        return ret;
+        return ath10k_ce_num_free_src_entries(ar_pci->pipe_info[pipe].ce_hdl);
 }
 
 static void ath10k_pci_hif_dump_area(struct ath10k *ar)
@@ -863,7 +846,6 @@ static int ath10k_pci_start_ce(struct ath10k *ar)
                                                    ath10k_pci_ce_send_done,
                                                    disable_interrupts);
                         completions += attr->src_nentries;
-                        pipe_info->num_sends_allowed = attr->src_nentries - 1;
                 }
 
                 if (attr->dest_nentries) {
@@ -1033,7 +1015,6 @@ static void ath10k_pci_process_ce(struct ath10k *ar)
                  */
                 spin_lock_bh(&compl->pipe_info->pipe_lock);
                 list_add_tail(&compl->list, &compl->pipe_info->compl_free);
-                compl->pipe_info->num_sends_allowed += send_done;
                 spin_unlock_bh(&compl->pipe_info->pipe_lock);
         }
 
diff --git a/drivers/net/wireless/ath/ath10k/pci.h b/drivers/net/wireless/ath/ath10k/pci.h
index 7c49f6f96f70..52fb7b973571 100644
--- a/drivers/net/wireless/ath/ath10k/pci.h
+++ b/drivers/net/wireless/ath/ath10k/pci.h
@@ -178,9 +178,6 @@ struct ath10k_pci_pipe {
         /* List of free CE completion slots */
         struct list_head compl_free;
 
-        /* Limit the number of outstanding send requests. */
-        int num_sends_allowed;
-
         struct ath10k_pci *ar_pci;
         struct tasklet_struct intr;
 };
diff --git a/drivers/net/wireless/ath/ath10k/wmi.c b/drivers/net/wireless/ath/ath10k/wmi.c
index 33cb19eb3d89..ccf3597fd9e2 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.c
+++ b/drivers/net/wireless/ath/ath10k/wmi.c
@@ -23,6 +23,471 @@
 #include "wmi.h"
 #include "mac.h"
 
+/* MAIN WMI cmd track */
+static struct wmi_cmd_map wmi_cmd_map = {
+        .init_cmdid = WMI_INIT_CMDID,
+        .start_scan_cmdid = WMI_START_SCAN_CMDID,
+        .stop_scan_cmdid = WMI_STOP_SCAN_CMDID,
+        .scan_chan_list_cmdid = WMI_SCAN_CHAN_LIST_CMDID,
+        .scan_sch_prio_tbl_cmdid = WMI_SCAN_SCH_PRIO_TBL_CMDID,
+        .pdev_set_regdomain_cmdid = WMI_PDEV_SET_REGDOMAIN_CMDID,
+        .pdev_set_channel_cmdid = WMI_PDEV_SET_CHANNEL_CMDID,
+        .pdev_set_param_cmdid = WMI_PDEV_SET_PARAM_CMDID,
+        .pdev_pktlog_enable_cmdid = WMI_PDEV_PKTLOG_ENABLE_CMDID,
+        .pdev_pktlog_disable_cmdid = WMI_PDEV_PKTLOG_DISABLE_CMDID,
+        .pdev_set_wmm_params_cmdid = WMI_PDEV_SET_WMM_PARAMS_CMDID,
+        .pdev_set_ht_cap_ie_cmdid = WMI_PDEV_SET_HT_CAP_IE_CMDID,
+        .pdev_set_vht_cap_ie_cmdid = WMI_PDEV_SET_VHT_CAP_IE_CMDID,
+        .pdev_set_dscp_tid_map_cmdid = WMI_PDEV_SET_DSCP_TID_MAP_CMDID,
+        .pdev_set_quiet_mode_cmdid = WMI_PDEV_SET_QUIET_MODE_CMDID,
+        .pdev_green_ap_ps_enable_cmdid = WMI_PDEV_GREEN_AP_PS_ENABLE_CMDID,
+        .pdev_get_tpc_config_cmdid = WMI_PDEV_GET_TPC_CONFIG_CMDID,
+        .pdev_set_base_macaddr_cmdid = WMI_PDEV_SET_BASE_MACADDR_CMDID,
+        .vdev_create_cmdid = WMI_VDEV_CREATE_CMDID,
+        .vdev_delete_cmdid = WMI_VDEV_DELETE_CMDID,
+        .vdev_start_request_cmdid = WMI_VDEV_START_REQUEST_CMDID,
+        .vdev_restart_request_cmdid = WMI_VDEV_RESTART_REQUEST_CMDID,
+        .vdev_up_cmdid = WMI_VDEV_UP_CMDID,
+        .vdev_stop_cmdid = WMI_VDEV_STOP_CMDID,
+        .vdev_down_cmdid = WMI_VDEV_DOWN_CMDID,
+        .vdev_set_param_cmdid = WMI_VDEV_SET_PARAM_CMDID,
+        .vdev_install_key_cmdid = WMI_VDEV_INSTALL_KEY_CMDID,
+        .peer_create_cmdid = WMI_PEER_CREATE_CMDID,
+        .peer_delete_cmdid = WMI_PEER_DELETE_CMDID,
+        .peer_flush_tids_cmdid = WMI_PEER_FLUSH_TIDS_CMDID,
+        .peer_set_param_cmdid = WMI_PEER_SET_PARAM_CMDID,
+        .peer_assoc_cmdid = WMI_PEER_ASSOC_CMDID,
+        .peer_add_wds_entry_cmdid = WMI_PEER_ADD_WDS_ENTRY_CMDID,
+        .peer_remove_wds_entry_cmdid = WMI_PEER_REMOVE_WDS_ENTRY_CMDID,
+        .peer_mcast_group_cmdid = WMI_PEER_MCAST_GROUP_CMDID,
+        .bcn_tx_cmdid = WMI_BCN_TX_CMDID,
+        .pdev_send_bcn_cmdid = WMI_PDEV_SEND_BCN_CMDID,
+        .bcn_tmpl_cmdid = WMI_BCN_TMPL_CMDID,
+        .bcn_filter_rx_cmdid = WMI_BCN_FILTER_RX_CMDID,
+        .prb_req_filter_rx_cmdid = WMI_PRB_REQ_FILTER_RX_CMDID,
+        .mgmt_tx_cmdid = WMI_MGMT_TX_CMDID,
+        .prb_tmpl_cmdid = WMI_PRB_TMPL_CMDID,
+        .addba_clear_resp_cmdid = WMI_ADDBA_CLEAR_RESP_CMDID,
+        .addba_send_cmdid = WMI_ADDBA_SEND_CMDID,
+        .addba_status_cmdid = WMI_ADDBA_STATUS_CMDID,
+        .delba_send_cmdid = WMI_DELBA_SEND_CMDID,
+        .addba_set_resp_cmdid = WMI_ADDBA_SET_RESP_CMDID,
+        .send_singleamsdu_cmdid = WMI_SEND_SINGLEAMSDU_CMDID,
+        .sta_powersave_mode_cmdid = WMI_STA_POWERSAVE_MODE_CMDID,
+        .sta_powersave_param_cmdid = WMI_STA_POWERSAVE_PARAM_CMDID,
+        .sta_mimo_ps_mode_cmdid = WMI_STA_MIMO_PS_MODE_CMDID,
+        .pdev_dfs_enable_cmdid = WMI_PDEV_DFS_ENABLE_CMDID,
+        .pdev_dfs_disable_cmdid = WMI_PDEV_DFS_DISABLE_CMDID,
+        .roam_scan_mode = WMI_ROAM_SCAN_MODE,
+        .roam_scan_rssi_threshold = WMI_ROAM_SCAN_RSSI_THRESHOLD,
+        .roam_scan_period = WMI_ROAM_SCAN_PERIOD,
+        .roam_scan_rssi_change_threshold = WMI_ROAM_SCAN_RSSI_CHANGE_THRESHOLD,
+        .roam_ap_profile = WMI_ROAM_AP_PROFILE,
+        .ofl_scan_add_ap_profile = WMI_ROAM_AP_PROFILE,
+        .ofl_scan_remove_ap_profile = WMI_OFL_SCAN_REMOVE_AP_PROFILE,
+        .ofl_scan_period = WMI_OFL_SCAN_PERIOD,
+        .p2p_dev_set_device_info = WMI_P2P_DEV_SET_DEVICE_INFO,
+        .p2p_dev_set_discoverability = WMI_P2P_DEV_SET_DISCOVERABILITY,
+        .p2p_go_set_beacon_ie = WMI_P2P_GO_SET_BEACON_IE,
+        .p2p_go_set_probe_resp_ie = WMI_P2P_GO_SET_PROBE_RESP_IE,
+        .p2p_set_vendor_ie_data_cmdid = WMI_P2P_SET_VENDOR_IE_DATA_CMDID,
+        .ap_ps_peer_param_cmdid = WMI_AP_PS_PEER_PARAM_CMDID,
+        .ap_ps_peer_uapsd_coex_cmdid = WMI_AP_PS_PEER_UAPSD_COEX_CMDID,
+        .peer_rate_retry_sched_cmdid = WMI_PEER_RATE_RETRY_SCHED_CMDID,
+        .wlan_profile_trigger_cmdid = WMI_WLAN_PROFILE_TRIGGER_CMDID,
+        .wlan_profile_set_hist_intvl_cmdid =
+                                WMI_WLAN_PROFILE_SET_HIST_INTVL_CMDID,
+        .wlan_profile_get_profile_data_cmdid =
+                                WMI_WLAN_PROFILE_GET_PROFILE_DATA_CMDID,
+        .wlan_profile_enable_profile_id_cmdid =
+                                WMI_WLAN_PROFILE_ENABLE_PROFILE_ID_CMDID,
+        .wlan_profile_list_profile_id_cmdid =
+                                WMI_WLAN_PROFILE_LIST_PROFILE_ID_CMDID,
+        .pdev_suspend_cmdid = WMI_PDEV_SUSPEND_CMDID,
+        .pdev_resume_cmdid = WMI_PDEV_RESUME_CMDID,
+        .add_bcn_filter_cmdid = WMI_ADD_BCN_FILTER_CMDID,
+        .rmv_bcn_filter_cmdid = WMI_RMV_BCN_FILTER_CMDID,
+        .wow_add_wake_pattern_cmdid = WMI_WOW_ADD_WAKE_PATTERN_CMDID,
+        .wow_del_wake_pattern_cmdid = WMI_WOW_DEL_WAKE_PATTERN_CMDID,
+        .wow_enable_disable_wake_event_cmdid =
+                                WMI_WOW_ENABLE_DISABLE_WAKE_EVENT_CMDID,
+        .wow_enable_cmdid = WMI_WOW_ENABLE_CMDID,
+        .wow_hostwakeup_from_sleep_cmdid = WMI_WOW_HOSTWAKEUP_FROM_SLEEP_CMDID,
+        .rtt_measreq_cmdid = WMI_RTT_MEASREQ_CMDID,
+        .rtt_tsf_cmdid = WMI_RTT_TSF_CMDID,
+        .vdev_spectral_scan_configure_cmdid =
+                                WMI_VDEV_SPECTRAL_SCAN_CONFIGURE_CMDID,
+        .vdev_spectral_scan_enable_cmdid = WMI_VDEV_SPECTRAL_SCAN_ENABLE_CMDID,
+        .request_stats_cmdid = WMI_REQUEST_STATS_CMDID,
+        .set_arp_ns_offload_cmdid = WMI_SET_ARP_NS_OFFLOAD_CMDID,
+        .network_list_offload_config_cmdid =
+                                WMI_NETWORK_LIST_OFFLOAD_CONFIG_CMDID,
+        .gtk_offload_cmdid = WMI_GTK_OFFLOAD_CMDID,
+        .csa_offload_enable_cmdid = WMI_CSA_OFFLOAD_ENABLE_CMDID,
+        .csa_offload_chanswitch_cmdid = WMI_CSA_OFFLOAD_CHANSWITCH_CMDID,
+        .chatter_set_mode_cmdid = WMI_CHATTER_SET_MODE_CMDID,
+        .peer_tid_addba_cmdid = WMI_PEER_TID_ADDBA_CMDID,
+        .peer_tid_delba_cmdid = WMI_PEER_TID_DELBA_CMDID,
+        .sta_dtim_ps_method_cmdid = WMI_STA_DTIM_PS_METHOD_CMDID,
+        .sta_uapsd_auto_trig_cmdid = WMI_STA_UAPSD_AUTO_TRIG_CMDID,
+        .sta_keepalive_cmd = WMI_STA_KEEPALIVE_CMD,
+        .echo_cmdid = WMI_ECHO_CMDID,
+        .pdev_utf_cmdid = WMI_PDEV_UTF_CMDID,
+        .dbglog_cfg_cmdid = WMI_DBGLOG_CFG_CMDID,
+        .pdev_qvit_cmdid = WMI_PDEV_QVIT_CMDID,
+        .pdev_ftm_intg_cmdid = WMI_PDEV_FTM_INTG_CMDID,
+        .vdev_set_keepalive_cmdid = WMI_VDEV_SET_KEEPALIVE_CMDID,
+        .vdev_get_keepalive_cmdid = WMI_VDEV_GET_KEEPALIVE_CMDID,
+        .force_fw_hang_cmdid = WMI_FORCE_FW_HANG_CMDID,
+        .gpio_config_cmdid = WMI_GPIO_CONFIG_CMDID,
+        .gpio_output_cmdid = WMI_GPIO_OUTPUT_CMDID,
+};
+
+/* 10.X WMI cmd track */
+static struct wmi_cmd_map wmi_10x_cmd_map = {
+        .init_cmdid = WMI_10X_INIT_CMDID,
+        .start_scan_cmdid = WMI_10X_START_SCAN_CMDID,
+        .stop_scan_cmdid = WMI_10X_STOP_SCAN_CMDID,
+        .scan_chan_list_cmdid = WMI_10X_SCAN_CHAN_LIST_CMDID,
+        .scan_sch_prio_tbl_cmdid = WMI_CMD_UNSUPPORTED,
+        .pdev_set_regdomain_cmdid = WMI_10X_PDEV_SET_REGDOMAIN_CMDID,
+        .pdev_set_channel_cmdid = WMI_10X_PDEV_SET_CHANNEL_CMDID,
+        .pdev_set_param_cmdid = WMI_10X_PDEV_SET_PARAM_CMDID,
+        .pdev_pktlog_enable_cmdid = WMI_10X_PDEV_PKTLOG_ENABLE_CMDID,
+        .pdev_pktlog_disable_cmdid = WMI_10X_PDEV_PKTLOG_DISABLE_CMDID,
+        .pdev_set_wmm_params_cmdid = WMI_10X_PDEV_SET_WMM_PARAMS_CMDID,
+        .pdev_set_ht_cap_ie_cmdid = WMI_10X_PDEV_SET_HT_CAP_IE_CMDID,
+        .pdev_set_vht_cap_ie_cmdid = WMI_10X_PDEV_SET_VHT_CAP_IE_CMDID,
+        .pdev_set_dscp_tid_map_cmdid = WMI_10X_PDEV_SET_DSCP_TID_MAP_CMDID,
+        .pdev_set_quiet_mode_cmdid = WMI_10X_PDEV_SET_QUIET_MODE_CMDID,
+        .pdev_green_ap_ps_enable_cmdid = WMI_10X_PDEV_GREEN_AP_PS_ENABLE_CMDID,
+        .pdev_get_tpc_config_cmdid = WMI_10X_PDEV_GET_TPC_CONFIG_CMDID,
+        .pdev_set_base_macaddr_cmdid = WMI_10X_PDEV_SET_BASE_MACADDR_CMDID,
+        .vdev_create_cmdid = WMI_10X_VDEV_CREATE_CMDID,
+        .vdev_delete_cmdid = WMI_10X_VDEV_DELETE_CMDID,
+        .vdev_start_request_cmdid = WMI_10X_VDEV_START_REQUEST_CMDID,
+        .vdev_restart_request_cmdid = WMI_10X_VDEV_RESTART_REQUEST_CMDID,
+        .vdev_up_cmdid = WMI_10X_VDEV_UP_CMDID,
+        .vdev_stop_cmdid = WMI_10X_VDEV_STOP_CMDID,
+        .vdev_down_cmdid = WMI_10X_VDEV_DOWN_CMDID,
+        .vdev_set_param_cmdid = WMI_10X_VDEV_SET_PARAM_CMDID,
+        .vdev_install_key_cmdid = WMI_10X_VDEV_INSTALL_KEY_CMDID,
+        .peer_create_cmdid = WMI_10X_PEER_CREATE_CMDID,
+        .peer_delete_cmdid = WMI_10X_PEER_DELETE_CMDID,
+        .peer_flush_tids_cmdid = WMI_10X_PEER_FLUSH_TIDS_CMDID,
+        .peer_set_param_cmdid = WMI_10X_PEER_SET_PARAM_CMDID,
+        .peer_assoc_cmdid = WMI_10X_PEER_ASSOC_CMDID,
+        .peer_add_wds_entry_cmdid = WMI_10X_PEER_ADD_WDS_ENTRY_CMDID,
+        .peer_remove_wds_entry_cmdid = WMI_10X_PEER_REMOVE_WDS_ENTRY_CMDID,
+        .peer_mcast_group_cmdid = WMI_10X_PEER_MCAST_GROUP_CMDID,
+        .bcn_tx_cmdid = WMI_10X_BCN_TX_CMDID,
+        .pdev_send_bcn_cmdid = WMI_10X_PDEV_SEND_BCN_CMDID,
+        .bcn_tmpl_cmdid = WMI_CMD_UNSUPPORTED,
+        .bcn_filter_rx_cmdid = WMI_10X_BCN_FILTER_RX_CMDID,
+        .prb_req_filter_rx_cmdid = WMI_10X_PRB_REQ_FILTER_RX_CMDID,
+        .mgmt_tx_cmdid = WMI_10X_MGMT_TX_CMDID,
+        .prb_tmpl_cmdid = WMI_CMD_UNSUPPORTED,
+        .addba_clear_resp_cmdid = WMI_10X_ADDBA_CLEAR_RESP_CMDID,
+        .addba_send_cmdid = WMI_10X_ADDBA_SEND_CMDID,
+        .addba_status_cmdid = WMI_10X_ADDBA_STATUS_CMDID,
+        .delba_send_cmdid = WMI_10X_DELBA_SEND_CMDID,
+        .addba_set_resp_cmdid = WMI_10X_ADDBA_SET_RESP_CMDID,
+        .send_singleamsdu_cmdid = WMI_10X_SEND_SINGLEAMSDU_CMDID,
+        .sta_powersave_mode_cmdid = WMI_10X_STA_POWERSAVE_MODE_CMDID,
+        .sta_powersave_param_cmdid = WMI_10X_STA_POWERSAVE_PARAM_CMDID,
+        .sta_mimo_ps_mode_cmdid = WMI_10X_STA_MIMO_PS_MODE_CMDID,
+        .pdev_dfs_enable_cmdid = WMI_10X_PDEV_DFS_ENABLE_CMDID,
+        .pdev_dfs_disable_cmdid = WMI_10X_PDEV_DFS_DISABLE_CMDID,
+        .roam_scan_mode = WMI_10X_ROAM_SCAN_MODE,
+        .roam_scan_rssi_threshold = WMI_10X_ROAM_SCAN_RSSI_THRESHOLD,
+        .roam_scan_period = WMI_10X_ROAM_SCAN_PERIOD,
+        .roam_scan_rssi_change_threshold =
+                                WMI_10X_ROAM_SCAN_RSSI_CHANGE_THRESHOLD,
+        .roam_ap_profile = WMI_10X_ROAM_AP_PROFILE,
+        .ofl_scan_add_ap_profile = WMI_10X_OFL_SCAN_ADD_AP_PROFILE,
+        .ofl_scan_remove_ap_profile = WMI_10X_OFL_SCAN_REMOVE_AP_PROFILE,
+        .ofl_scan_period = WMI_10X_OFL_SCAN_PERIOD,
+        .p2p_dev_set_device_info = WMI_10X_P2P_DEV_SET_DEVICE_INFO,
+        .p2p_dev_set_discoverability = WMI_10X_P2P_DEV_SET_DISCOVERABILITY,
+        .p2p_go_set_beacon_ie = WMI_10X_P2P_GO_SET_BEACON_IE,
+        .p2p_go_set_probe_resp_ie = WMI_10X_P2P_GO_SET_PROBE_RESP_IE,
+        .p2p_set_vendor_ie_data_cmdid = WMI_CMD_UNSUPPORTED,
+        .ap_ps_peer_param_cmdid = WMI_CMD_UNSUPPORTED,
+        .ap_ps_peer_uapsd_coex_cmdid = WMI_CMD_UNSUPPORTED,
+        .peer_rate_retry_sched_cmdid = WMI_10X_PEER_RATE_RETRY_SCHED_CMDID,
+        .wlan_profile_trigger_cmdid = WMI_10X_WLAN_PROFILE_TRIGGER_CMDID,
+        .wlan_profile_set_hist_intvl_cmdid =
+                                WMI_10X_WLAN_PROFILE_SET_HIST_INTVL_CMDID,
+        .wlan_profile_get_profile_data_cmdid =
+                                WMI_10X_WLAN_PROFILE_GET_PROFILE_DATA_CMDID,
+        .wlan_profile_enable_profile_id_cmdid =
+                                WMI_10X_WLAN_PROFILE_ENABLE_PROFILE_ID_CMDID,
+        .wlan_profile_list_profile_id_cmdid =
+                                WMI_10X_WLAN_PROFILE_LIST_PROFILE_ID_CMDID,
+        .pdev_suspend_cmdid = WMI_10X_PDEV_SUSPEND_CMDID,
+        .pdev_resume_cmdid = WMI_10X_PDEV_RESUME_CMDID,
+        .add_bcn_filter_cmdid = WMI_10X_ADD_BCN_FILTER_CMDID,
+        .rmv_bcn_filter_cmdid = WMI_10X_RMV_BCN_FILTER_CMDID,
+        .wow_add_wake_pattern_cmdid = WMI_10X_WOW_ADD_WAKE_PATTERN_CMDID,
+        .wow_del_wake_pattern_cmdid = WMI_10X_WOW_DEL_WAKE_PATTERN_CMDID,
+        .wow_enable_disable_wake_event_cmdid =
+                                WMI_10X_WOW_ENABLE_DISABLE_WAKE_EVENT_CMDID,
+        .wow_enable_cmdid = WMI_10X_WOW_ENABLE_CMDID,
+        .wow_hostwakeup_from_sleep_cmdid =
+                                WMI_10X_WOW_HOSTWAKEUP_FROM_SLEEP_CMDID,
+        .rtt_measreq_cmdid = WMI_10X_RTT_MEASREQ_CMDID,
+        .rtt_tsf_cmdid = WMI_10X_RTT_TSF_CMDID,
+        .vdev_spectral_scan_configure_cmdid =
+                                WMI_10X_VDEV_SPECTRAL_SCAN_CONFIGURE_CMDID,
+        .vdev_spectral_scan_enable_cmdid =
+                                WMI_10X_VDEV_SPECTRAL_SCAN_ENABLE_CMDID,
+        .request_stats_cmdid = WMI_10X_REQUEST_STATS_CMDID,
+        .set_arp_ns_offload_cmdid = WMI_CMD_UNSUPPORTED,
+        .network_list_offload_config_cmdid = WMI_CMD_UNSUPPORTED,
+        .gtk_offload_cmdid = WMI_CMD_UNSUPPORTED,
+        .csa_offload_enable_cmdid = WMI_CMD_UNSUPPORTED,
+        .csa_offload_chanswitch_cmdid = WMI_CMD_UNSUPPORTED,
+        .chatter_set_mode_cmdid = WMI_CMD_UNSUPPORTED,
+        .peer_tid_addba_cmdid = WMI_CMD_UNSUPPORTED,
+        .peer_tid_delba_cmdid = WMI_CMD_UNSUPPORTED,
+        .sta_dtim_ps_method_cmdid = WMI_CMD_UNSUPPORTED,
+        .sta_uapsd_auto_trig_cmdid = WMI_CMD_UNSUPPORTED,
+        .sta_keepalive_cmd = WMI_CMD_UNSUPPORTED,
+        .echo_cmdid = WMI_10X_ECHO_CMDID,
+        .pdev_utf_cmdid = WMI_10X_PDEV_UTF_CMDID,
+        .dbglog_cfg_cmdid = WMI_10X_DBGLOG_CFG_CMDID,
+        .pdev_qvit_cmdid = WMI_10X_PDEV_QVIT_CMDID,
+        .pdev_ftm_intg_cmdid = WMI_CMD_UNSUPPORTED,
+        .vdev_set_keepalive_cmdid = WMI_CMD_UNSUPPORTED,
+        .vdev_get_keepalive_cmdid = WMI_CMD_UNSUPPORTED,
+        .force_fw_hang_cmdid = WMI_CMD_UNSUPPORTED,
+        .gpio_config_cmdid = WMI_10X_GPIO_CONFIG_CMDID,
+        .gpio_output_cmdid = WMI_10X_GPIO_OUTPUT_CMDID,
+};
+
+/* MAIN WMI VDEV param map */
+static struct wmi_vdev_param_map wmi_vdev_param_map = {
+        .rts_threshold = WMI_VDEV_PARAM_RTS_THRESHOLD,
+        .fragmentation_threshold = WMI_VDEV_PARAM_FRAGMENTATION_THRESHOLD,
+        .beacon_interval = WMI_VDEV_PARAM_BEACON_INTERVAL,
+        .listen_interval = WMI_VDEV_PARAM_LISTEN_INTERVAL,
+        .multicast_rate = WMI_VDEV_PARAM_MULTICAST_RATE,
+        .mgmt_tx_rate = WMI_VDEV_PARAM_MGMT_TX_RATE,
+        .slot_time = WMI_VDEV_PARAM_SLOT_TIME,
+        .preamble = WMI_VDEV_PARAM_PREAMBLE,
+        .swba_time = WMI_VDEV_PARAM_SWBA_TIME,
+        .wmi_vdev_stats_update_period = WMI_VDEV_STATS_UPDATE_PERIOD,
+        .wmi_vdev_pwrsave_ageout_time = WMI_VDEV_PWRSAVE_AGEOUT_TIME,
+        .wmi_vdev_host_swba_interval = WMI_VDEV_HOST_SWBA_INTERVAL,
+        .dtim_period = WMI_VDEV_PARAM_DTIM_PERIOD,
+        .wmi_vdev_oc_scheduler_air_time_limit =
+                                        WMI_VDEV_OC_SCHEDULER_AIR_TIME_LIMIT,
+        .wds = WMI_VDEV_PARAM_WDS,
+        .atim_window = WMI_VDEV_PARAM_ATIM_WINDOW,
+        .bmiss_count_max = WMI_VDEV_PARAM_BMISS_COUNT_MAX,
+        .bmiss_first_bcnt = WMI_VDEV_PARAM_BMISS_FIRST_BCNT,
+        .bmiss_final_bcnt = WMI_VDEV_PARAM_BMISS_FINAL_BCNT,
+        .feature_wmm = WMI_VDEV_PARAM_FEATURE_WMM,
+        .chwidth = WMI_VDEV_PARAM_CHWIDTH,
+        .chextoffset = WMI_VDEV_PARAM_CHEXTOFFSET,
+        .disable_htprotection = WMI_VDEV_PARAM_DISABLE_HTPROTECTION,
+        .sta_quickkickout = WMI_VDEV_PARAM_STA_QUICKKICKOUT,
+        .mgmt_rate = WMI_VDEV_PARAM_MGMT_RATE,
+        .protection_mode = WMI_VDEV_PARAM_PROTECTION_MODE,
+        .fixed_rate = WMI_VDEV_PARAM_FIXED_RATE,
+        .sgi = WMI_VDEV_PARAM_SGI,
+        .ldpc = WMI_VDEV_PARAM_LDPC,
+        .tx_stbc = WMI_VDEV_PARAM_TX_STBC,
+        .rx_stbc = WMI_VDEV_PARAM_RX_STBC,
+        .intra_bss_fwd = WMI_VDEV_PARAM_INTRA_BSS_FWD,
+        .def_keyid = WMI_VDEV_PARAM_DEF_KEYID,
+        .nss = WMI_VDEV_PARAM_NSS,
+        .bcast_data_rate = WMI_VDEV_PARAM_BCAST_DATA_RATE,
+        .mcast_data_rate = WMI_VDEV_PARAM_MCAST_DATA_RATE,
+        .mcast_indicate = WMI_VDEV_PARAM_MCAST_INDICATE,
+        .dhcp_indicate = WMI_VDEV_PARAM_DHCP_INDICATE,
+        .unknown_dest_indicate = WMI_VDEV_PARAM_UNKNOWN_DEST_INDICATE,
+        .ap_keepalive_min_idle_inactive_time_secs =
+                        WMI_VDEV_PARAM_AP_KEEPALIVE_MIN_IDLE_INACTIVE_TIME_SECS,
+        .ap_keepalive_max_idle_inactive_time_secs =
+                        WMI_VDEV_PARAM_AP_KEEPALIVE_MAX_IDLE_INACTIVE_TIME_SECS,
+        .ap_keepalive_max_unresponsive_time_secs =
+                        WMI_VDEV_PARAM_AP_KEEPALIVE_MAX_UNRESPONSIVE_TIME_SECS,
+        .ap_enable_nawds = WMI_VDEV_PARAM_AP_ENABLE_NAWDS,
+        .mcast2ucast_set = WMI_VDEV_PARAM_UNSUPPORTED,
+        .enable_rtscts = WMI_VDEV_PARAM_ENABLE_RTSCTS,
+        .txbf = WMI_VDEV_PARAM_TXBF,
+        .packet_powersave = WMI_VDEV_PARAM_PACKET_POWERSAVE,
+        .drop_unencry = WMI_VDEV_PARAM_DROP_UNENCRY,
+        .tx_encap_type = WMI_VDEV_PARAM_TX_ENCAP_TYPE,
+        .ap_detect_out_of_sync_sleeping_sta_time_secs =
+                                        WMI_VDEV_PARAM_UNSUPPORTED,
+};
+
+/* 10.X WMI VDEV param map */
+static struct wmi_vdev_param_map wmi_10x_vdev_param_map = {
+        .rts_threshold = WMI_10X_VDEV_PARAM_RTS_THRESHOLD,
+        .fragmentation_threshold = WMI_10X_VDEV_PARAM_FRAGMENTATION_THRESHOLD,
+        .beacon_interval = WMI_10X_VDEV_PARAM_BEACON_INTERVAL,
+        .listen_interval = WMI_10X_VDEV_PARAM_LISTEN_INTERVAL,
+        .multicast_rate = WMI_10X_VDEV_PARAM_MULTICAST_RATE,
+        .mgmt_tx_rate = WMI_10X_VDEV_PARAM_MGMT_TX_RATE,
+        .slot_time = WMI_10X_VDEV_PARAM_SLOT_TIME,
+        .preamble = WMI_10X_VDEV_PARAM_PREAMBLE,
+        .swba_time = WMI_10X_VDEV_PARAM_SWBA_TIME,
+        .wmi_vdev_stats_update_period = WMI_10X_VDEV_STATS_UPDATE_PERIOD,
+        .wmi_vdev_pwrsave_ageout_time = WMI_10X_VDEV_PWRSAVE_AGEOUT_TIME,
+        .wmi_vdev_host_swba_interval = WMI_10X_VDEV_HOST_SWBA_INTERVAL,
+        .dtim_period = WMI_10X_VDEV_PARAM_DTIM_PERIOD,
+        .wmi_vdev_oc_scheduler_air_time_limit =
+                                WMI_10X_VDEV_OC_SCHEDULER_AIR_TIME_LIMIT,
+        .wds = WMI_10X_VDEV_PARAM_WDS,
+        .atim_window = WMI_10X_VDEV_PARAM_ATIM_WINDOW,
+        .bmiss_count_max = WMI_10X_VDEV_PARAM_BMISS_COUNT_MAX,
+        .bmiss_first_bcnt = WMI_VDEV_PARAM_UNSUPPORTED,
+        .bmiss_final_bcnt = WMI_VDEV_PARAM_UNSUPPORTED,
+        .feature_wmm = WMI_10X_VDEV_PARAM_FEATURE_WMM,
+        .chwidth = WMI_10X_VDEV_PARAM_CHWIDTH,
+        .chextoffset = WMI_10X_VDEV_PARAM_CHEXTOFFSET,
+        .disable_htprotection = WMI_10X_VDEV_PARAM_DISABLE_HTPROTECTION,
+        .sta_quickkickout = WMI_10X_VDEV_PARAM_STA_QUICKKICKOUT,
+        .mgmt_rate = WMI_10X_VDEV_PARAM_MGMT_RATE,
+        .protection_mode = WMI_10X_VDEV_PARAM_PROTECTION_MODE,
+        .fixed_rate = WMI_10X_VDEV_PARAM_FIXED_RATE,
+        .sgi = WMI_10X_VDEV_PARAM_SGI,
+        .ldpc = WMI_10X_VDEV_PARAM_LDPC,
+        .tx_stbc = WMI_10X_VDEV_PARAM_TX_STBC,
+        .rx_stbc = WMI_10X_VDEV_PARAM_RX_STBC,
+        .intra_bss_fwd = WMI_10X_VDEV_PARAM_INTRA_BSS_FWD,
+        .def_keyid = WMI_10X_VDEV_PARAM_DEF_KEYID,
+        .nss = WMI_10X_VDEV_PARAM_NSS,
+        .bcast_data_rate = WMI_10X_VDEV_PARAM_BCAST_DATA_RATE,
+        .mcast_data_rate = WMI_10X_VDEV_PARAM_MCAST_DATA_RATE,
+        .mcast_indicate = WMI_10X_VDEV_PARAM_MCAST_INDICATE,
+        .dhcp_indicate = WMI_10X_VDEV_PARAM_DHCP_INDICATE,
+        .unknown_dest_indicate = WMI_10X_VDEV_PARAM_UNKNOWN_DEST_INDICATE,
+        .ap_keepalive_min_idle_inactive_time_secs =
+                WMI_10X_VDEV_PARAM_AP_KEEPALIVE_MIN_IDLE_INACTIVE_TIME_SECS,
+        .ap_keepalive_max_idle_inactive_time_secs =
+                WMI_10X_VDEV_PARAM_AP_KEEPALIVE_MAX_IDLE_INACTIVE_TIME_SECS,
+        .ap_keepalive_max_unresponsive_time_secs =
+                WMI_10X_VDEV_PARAM_AP_KEEPALIVE_MAX_UNRESPONSIVE_TIME_SECS,
+        .ap_enable_nawds = WMI_10X_VDEV_PARAM_AP_ENABLE_NAWDS,
+        .mcast2ucast_set = WMI_10X_VDEV_PARAM_MCAST2UCAST_SET,
+        .enable_rtscts = WMI_10X_VDEV_PARAM_ENABLE_RTSCTS,
+        .txbf = WMI_VDEV_PARAM_UNSUPPORTED,
+        .packet_powersave = WMI_VDEV_PARAM_UNSUPPORTED,
+        .drop_unencry = WMI_VDEV_PARAM_UNSUPPORTED,
+        .tx_encap_type = WMI_VDEV_PARAM_UNSUPPORTED,
+        .ap_detect_out_of_sync_sleeping_sta_time_secs =
+                WMI_10X_VDEV_PARAM_AP_DETECT_OUT_OF_SYNC_SLEEPING_STA_TIME_SECS,
+};
+
+static struct wmi_pdev_param_map wmi_pdev_param_map = {
+        .tx_chain_mask = WMI_PDEV_PARAM_TX_CHAIN_MASK,
+        .rx_chain_mask = WMI_PDEV_PARAM_RX_CHAIN_MASK,
+        .txpower_limit2g = WMI_PDEV_PARAM_TXPOWER_LIMIT2G,
+        .txpower_limit5g = WMI_PDEV_PARAM_TXPOWER_LIMIT5G,
+        .txpower_scale = WMI_PDEV_PARAM_TXPOWER_SCALE,
+        .beacon_gen_mode = WMI_PDEV_PARAM_BEACON_GEN_MODE,
+        .beacon_tx_mode = WMI_PDEV_PARAM_BEACON_TX_MODE,
+        .resmgr_offchan_mode = WMI_PDEV_PARAM_RESMGR_OFFCHAN_MODE,
+        .protection_mode = WMI_PDEV_PARAM_PROTECTION_MODE,
+        .dynamic_bw = WMI_PDEV_PARAM_DYNAMIC_BW,
+        .non_agg_sw_retry_th = WMI_PDEV_PARAM_NON_AGG_SW_RETRY_TH,
+        .agg_sw_retry_th = WMI_PDEV_PARAM_AGG_SW_RETRY_TH,
+        .sta_kickout_th = WMI_PDEV_PARAM_STA_KICKOUT_TH,
+        .ac_aggrsize_scaling = WMI_PDEV_PARAM_AC_AGGRSIZE_SCALING,
+        .ltr_enable = WMI_PDEV_PARAM_LTR_ENABLE,
+        .ltr_ac_latency_be = WMI_PDEV_PARAM_LTR_AC_LATENCY_BE,
+        .ltr_ac_latency_bk = WMI_PDEV_PARAM_LTR_AC_LATENCY_BK,
+        .ltr_ac_latency_vi = WMI_PDEV_PARAM_LTR_AC_LATENCY_VI,
+        .ltr_ac_latency_vo = WMI_PDEV_PARAM_LTR_AC_LATENCY_VO,
+        .ltr_ac_latency_timeout = WMI_PDEV_PARAM_LTR_AC_LATENCY_TIMEOUT,
+        .ltr_sleep_override = WMI_PDEV_PARAM_LTR_SLEEP_OVERRIDE,
+        .ltr_rx_override = WMI_PDEV_PARAM_LTR_RX_OVERRIDE,
+        .ltr_tx_activity_timeout = WMI_PDEV_PARAM_LTR_TX_ACTIVITY_TIMEOUT,
+        .l1ss_enable = WMI_PDEV_PARAM_L1SS_ENABLE,
+        .dsleep_enable = WMI_PDEV_PARAM_DSLEEP_ENABLE,
+        .pcielp_txbuf_flush = WMI_PDEV_PARAM_PCIELP_TXBUF_FLUSH,
+        .pcielp_txbuf_watermark = WMI_PDEV_PARAM_PCIELP_TXBUF_TMO_EN,
+        .pcielp_txbuf_tmo_en = WMI_PDEV_PARAM_PCIELP_TXBUF_TMO_EN,
+        .pcielp_txbuf_tmo_value = WMI_PDEV_PARAM_PCIELP_TXBUF_TMO_VALUE,
+        .pdev_stats_update_period = WMI_PDEV_PARAM_PDEV_STATS_UPDATE_PERIOD,
+        .vdev_stats_update_period = WMI_PDEV_PARAM_VDEV_STATS_UPDATE_PERIOD,
+        .peer_stats_update_period = WMI_PDEV_PARAM_PEER_STATS_UPDATE_PERIOD,
+        .bcnflt_stats_update_period = WMI_PDEV_PARAM_BCNFLT_STATS_UPDATE_PERIOD,
+        .pmf_qos = WMI_PDEV_PARAM_PMF_QOS,
+        .arp_ac_override = WMI_PDEV_PARAM_ARP_AC_OVERRIDE,
+        .arpdhcp_ac_override = WMI_PDEV_PARAM_UNSUPPORTED,
+        .dcs = WMI_PDEV_PARAM_DCS,
+        .ani_enable = WMI_PDEV_PARAM_ANI_ENABLE,
+        .ani_poll_period = WMI_PDEV_PARAM_ANI_POLL_PERIOD,
+        .ani_listen_period = WMI_PDEV_PARAM_ANI_LISTEN_PERIOD,
+        .ani_ofdm_level = WMI_PDEV_PARAM_ANI_OFDM_LEVEL,
+        .ani_cck_level = WMI_PDEV_PARAM_ANI_CCK_LEVEL,
+        .dyntxchain = WMI_PDEV_PARAM_DYNTXCHAIN,
+        .proxy_sta = WMI_PDEV_PARAM_PROXY_STA,
+        .idle_ps_config = WMI_PDEV_PARAM_IDLE_PS_CONFIG,
+        .power_gating_sleep = WMI_PDEV_PARAM_POWER_GATING_SLEEP,
+        .fast_channel_reset = WMI_PDEV_PARAM_UNSUPPORTED,
+        .burst_dur = WMI_PDEV_PARAM_UNSUPPORTED,
+        .burst_enable = WMI_PDEV_PARAM_UNSUPPORTED,
+};
+
+static struct wmi_pdev_param_map wmi_10x_pdev_param_map = {
+        .tx_chain_mask = WMI_10X_PDEV_PARAM_TX_CHAIN_MASK,
+        .rx_chain_mask = WMI_10X_PDEV_PARAM_RX_CHAIN_MASK,
+        .txpower_limit2g = WMI_10X_PDEV_PARAM_TXPOWER_LIMIT2G,
+        .txpower_limit5g = WMI_10X_PDEV_PARAM_TXPOWER_LIMIT5G,
+        .txpower_scale = WMI_10X_PDEV_PARAM_TXPOWER_SCALE,
+        .beacon_gen_mode = WMI_10X_PDEV_PARAM_BEACON_GEN_MODE,
+        .beacon_tx_mode = WMI_10X_PDEV_PARAM_BEACON_TX_MODE,
+        .resmgr_offchan_mode = WMI_10X_PDEV_PARAM_RESMGR_OFFCHAN_MODE,
+        .protection_mode = WMI_10X_PDEV_PARAM_PROTECTION_MODE,
+        .dynamic_bw = WMI_10X_PDEV_PARAM_DYNAMIC_BW,
+        .non_agg_sw_retry_th = WMI_10X_PDEV_PARAM_NON_AGG_SW_RETRY_TH,
+        .agg_sw_retry_th = WMI_10X_PDEV_PARAM_AGG_SW_RETRY_TH,
+        .sta_kickout_th = WMI_10X_PDEV_PARAM_STA_KICKOUT_TH,
+        .ac_aggrsize_scaling = WMI_10X_PDEV_PARAM_AC_AGGRSIZE_SCALING,
+        .ltr_enable = WMI_10X_PDEV_PARAM_LTR_ENABLE,
+        .ltr_ac_latency_be = WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_BE,
+        .ltr_ac_latency_bk = WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_BK,
+        .ltr_ac_latency_vi = WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_VI,
+        .ltr_ac_latency_vo = WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_VO,
+        .ltr_ac_latency_timeout = WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_TIMEOUT,
+        .ltr_sleep_override = WMI_10X_PDEV_PARAM_LTR_SLEEP_OVERRIDE,
+        .ltr_rx_override = WMI_10X_PDEV_PARAM_LTR_RX_OVERRIDE,
+        .ltr_tx_activity_timeout = WMI_10X_PDEV_PARAM_LTR_TX_ACTIVITY_TIMEOUT,
+        .l1ss_enable = WMI_10X_PDEV_PARAM_L1SS_ENABLE,
+        .dsleep_enable = WMI_10X_PDEV_PARAM_DSLEEP_ENABLE,
+        .pcielp_txbuf_flush = WMI_PDEV_PARAM_UNSUPPORTED,
+        .pcielp_txbuf_watermark = WMI_PDEV_PARAM_UNSUPPORTED,
+        .pcielp_txbuf_tmo_en = WMI_PDEV_PARAM_UNSUPPORTED,
+        .pcielp_txbuf_tmo_value = WMI_PDEV_PARAM_UNSUPPORTED,
+        .pdev_stats_update_period = WMI_10X_PDEV_PARAM_PDEV_STATS_UPDATE_PERIOD,
+        .vdev_stats_update_period = WMI_10X_PDEV_PARAM_VDEV_STATS_UPDATE_PERIOD,
+        .peer_stats_update_period = WMI_10X_PDEV_PARAM_PEER_STATS_UPDATE_PERIOD,
+        .bcnflt_stats_update_period =
+                                WMI_10X_PDEV_PARAM_BCNFLT_STATS_UPDATE_PERIOD,
+        .pmf_qos = WMI_10X_PDEV_PARAM_PMF_QOS,
+        .arp_ac_override = WMI_PDEV_PARAM_UNSUPPORTED,
+        .arpdhcp_ac_override = WMI_10X_PDEV_PARAM_ARPDHCP_AC_OVERRIDE,
+        .dcs = WMI_10X_PDEV_PARAM_DCS,
+        .ani_enable = WMI_10X_PDEV_PARAM_ANI_ENABLE,
+        .ani_poll_period = WMI_10X_PDEV_PARAM_ANI_POLL_PERIOD,
+        .ani_listen_period = WMI_10X_PDEV_PARAM_ANI_LISTEN_PERIOD,
+        .ani_ofdm_level = WMI_10X_PDEV_PARAM_ANI_OFDM_LEVEL,
+        .ani_cck_level = WMI_10X_PDEV_PARAM_ANI_CCK_LEVEL,
+        .dyntxchain = WMI_10X_PDEV_PARAM_DYNTXCHAIN,
+        .proxy_sta = WMI_PDEV_PARAM_UNSUPPORTED,
+        .idle_ps_config = WMI_PDEV_PARAM_UNSUPPORTED,
+        .power_gating_sleep = WMI_PDEV_PARAM_UNSUPPORTED,
+        .fast_channel_reset = WMI_10X_PDEV_PARAM_FAST_CHANNEL_RESET,
+        .burst_dur = WMI_10X_PDEV_PARAM_BURST_DUR,
+        .burst_enable = WMI_10X_PDEV_PARAM_BURST_ENABLE,
+};
+
 int ath10k_wmi_wait_for_service_ready(struct ath10k *ar)
 {
         int ret;
@@ -64,7 +529,7 @@ static void ath10k_wmi_htc_tx_complete(struct ath10k *ar, struct sk_buff *skb)
 }
 
 static int ath10k_wmi_cmd_send_nowait(struct ath10k *ar, struct sk_buff *skb,
-                                      enum wmi_cmd_id cmd_id)
+                                      u32 cmd_id)
 {
         struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(skb);
         struct wmi_cmd_hdr *cmd_hdr;
@@ -144,9 +609,17 @@ static void ath10k_wmi_op_ep_tx_credits(struct ath10k *ar)
 }
 
 static int ath10k_wmi_cmd_send(struct ath10k *ar, struct sk_buff *skb,
-                               enum wmi_cmd_id cmd_id)
+                               u32 cmd_id)
 {
-        int ret = -EINVAL;
+        int ret = -EOPNOTSUPP;
+
+        might_sleep();
+
+        if (cmd_id == WMI_CMD_UNSUPPORTED) {
+                ath10k_warn("wmi command %d is not supported by firmware\n",
+                            cmd_id);
+                return ret;
+        }
 
         wait_event_timeout(ar->wmi.tx_credits_wq, ({
                 /* try to send pending beacons first. they take priority */
@@ -162,6 +635,57 @@ static int ath10k_wmi_cmd_send(struct ath10k *ar, struct sk_buff *skb,
         return ret;
 }
 
+int ath10k_wmi_mgmt_tx(struct ath10k *ar, struct sk_buff *skb)
+{
+        int ret = 0;
+        struct wmi_mgmt_tx_cmd *cmd;
+        struct ieee80211_hdr *hdr;
+        struct sk_buff *wmi_skb;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+        int len;
+        u16 fc;
+
+        hdr = (struct ieee80211_hdr *)skb->data;
+        fc = le16_to_cpu(hdr->frame_control);
+
+        if (WARN_ON_ONCE(!ieee80211_is_mgmt(hdr->frame_control)))
+                return -EINVAL;
+
+        len = sizeof(cmd->hdr) + skb->len;
+        len = round_up(len, 4);
+
+        wmi_skb = ath10k_wmi_alloc_skb(len);
+        if (!wmi_skb)
+                return -ENOMEM;
+
+        cmd = (struct wmi_mgmt_tx_cmd *)wmi_skb->data;
+
+        cmd->hdr.vdev_id = __cpu_to_le32(ATH10K_SKB_CB(skb)->vdev_id);
+        cmd->hdr.tx_rate = 0;
+        cmd->hdr.tx_power = 0;
+        cmd->hdr.buf_len = __cpu_to_le32((u32)(skb->len));
+
+        memcpy(cmd->hdr.peer_macaddr.addr, ieee80211_get_DA(hdr), ETH_ALEN);
+        memcpy(cmd->buf, skb->data, skb->len);
+
+        ath10k_dbg(ATH10K_DBG_WMI, "wmi mgmt tx skb %p len %d ftype %02x stype %02x\n",
+                   wmi_skb, wmi_skb->len, fc & IEEE80211_FCTL_FTYPE,
+                   fc & IEEE80211_FCTL_STYPE);
+
+        /* Send the management frame buffer to the target */
+        ret = ath10k_wmi_cmd_send(ar, wmi_skb, ar->wmi.cmd->mgmt_tx_cmdid);
+        if (ret) {
+                dev_kfree_skb_any(skb);
+                return ret;
+        }
+
+        /* TODO: report tx status to mac80211 - temporary just ACK */
+        info->flags |= IEEE80211_TX_STAT_ACK;
+        ieee80211_tx_status_irqsafe(ar->hw, skb);
+
+        return ret;
+}
+
 static int ath10k_wmi_event_scan(struct ath10k *ar, struct sk_buff *skb)
 {
         struct wmi_scan_event *event = (struct wmi_scan_event *)skb->data;
@@ -964,6 +1488,55 @@ static void ath10k_wmi_event_vdev_install_key_complete(struct ath10k *ar,
         ath10k_dbg(ATH10K_DBG_WMI, "WMI_VDEV_INSTALL_KEY_COMPLETE_EVENTID\n");
 }
 
+static void ath10k_wmi_event_inst_rssi_stats(struct ath10k *ar,
+                                             struct sk_buff *skb)
+{
+        ath10k_dbg(ATH10K_DBG_WMI, "WMI_INST_RSSI_STATS_EVENTID\n");
+}
+
+static void ath10k_wmi_event_vdev_standby_req(struct ath10k *ar,
+                                              struct sk_buff *skb)
+{
+        ath10k_dbg(ATH10K_DBG_WMI, "WMI_VDEV_STANDBY_REQ_EVENTID\n");
+}
+
+static void ath10k_wmi_event_vdev_resume_req(struct ath10k *ar,
+                                             struct sk_buff *skb)
+{
+        ath10k_dbg(ATH10K_DBG_WMI, "WMI_VDEV_RESUME_REQ_EVENTID\n");
+}
+
+static int ath10k_wmi_alloc_host_mem(struct ath10k *ar, u32 req_id,
+                                      u32 num_units, u32 unit_len)
+{
+        dma_addr_t paddr;
+        u32 pool_size;
+        int idx = ar->wmi.num_mem_chunks;
+
+        pool_size = num_units * round_up(unit_len, 4);
+
+        if (!pool_size)
+                return -EINVAL;
+
+        ar->wmi.mem_chunks[idx].vaddr = dma_alloc_coherent(ar->dev,
+                                                           pool_size,
+                                                           &paddr,
+                                                           GFP_ATOMIC);
+        if (!ar->wmi.mem_chunks[idx].vaddr) {
+                ath10k_warn("failed to allocate memory chunk\n");
+                return -ENOMEM;
+        }
+
+        memset(ar->wmi.mem_chunks[idx].vaddr, 0, pool_size);
+
+        ar->wmi.mem_chunks[idx].paddr = paddr;
+        ar->wmi.mem_chunks[idx].len = pool_size;
+        ar->wmi.mem_chunks[idx].req_id = req_id;
+        ar->wmi.num_mem_chunks++;
+
+        return 0;
+}
+
 static void ath10k_wmi_service_ready_event_rx(struct ath10k *ar,
                                               struct sk_buff *skb)
 {
@@ -988,7 +1561,8 @@ static void ath10k_wmi_service_ready_event_rx(struct ath10k *ar,
         ar->phy_capability = __le32_to_cpu(ev->phy_capability);
         ar->num_rf_chains = __le32_to_cpu(ev->num_rf_chains);
 
-        if (ar->fw_version_build > 636)
+        /* only manually set fw features when not using FW IE format */
+        if (ar->fw_api == 1 && ar->fw_version_build > 636)
                 set_bit(ATH10K_FW_FEATURE_EXT_WMI_MGMT_RX, ar->fw_features);
 
         if (ar->num_rf_chains > WMI_MAX_SPATIAL_STREAM) {
@@ -1035,6 +1609,108 @@ static void ath10k_wmi_service_ready_event_rx(struct ath10k *ar,
         complete(&ar->wmi.service_ready);
 }
 
+static void ath10k_wmi_10x_service_ready_event_rx(struct ath10k *ar,
+                                                  struct sk_buff *skb)
+{
+        u32 num_units, req_id, unit_size, num_mem_reqs, num_unit_info, i;
+        int ret;
+        struct wmi_service_ready_event_10x *ev = (void *)skb->data;
+
+        if (skb->len < sizeof(*ev)) {
+                ath10k_warn("Service ready event was %d B but expected %zu B. Wrong firmware version?\n",
+                            skb->len, sizeof(*ev));
+                return;
+        }
+
+        ar->hw_min_tx_power = __le32_to_cpu(ev->hw_min_tx_power);
+        ar->hw_max_tx_power = __le32_to_cpu(ev->hw_max_tx_power);
+        ar->ht_cap_info = __le32_to_cpu(ev->ht_cap_info);
+        ar->vht_cap_info = __le32_to_cpu(ev->vht_cap_info);
+        ar->fw_version_major =
+                (__le32_to_cpu(ev->sw_version) & 0xff000000) >> 24;
+        ar->fw_version_minor = (__le32_to_cpu(ev->sw_version) & 0x00ffffff);
+        ar->phy_capability = __le32_to_cpu(ev->phy_capability);
+        ar->num_rf_chains = __le32_to_cpu(ev->num_rf_chains);
+
+        if (ar->num_rf_chains > WMI_MAX_SPATIAL_STREAM) {
+                ath10k_warn("hardware advertises support for more spatial streams than it should (%d > %d)\n",
+                            ar->num_rf_chains, WMI_MAX_SPATIAL_STREAM);
+                ar->num_rf_chains = WMI_MAX_SPATIAL_STREAM;
+        }
+
+        ar->ath_common.regulatory.current_rd =
+                __le32_to_cpu(ev->hal_reg_capabilities.eeprom_rd);
+
+        ath10k_debug_read_service_map(ar, ev->wmi_service_bitmap,
+                                      sizeof(ev->wmi_service_bitmap));
+
+        if (strlen(ar->hw->wiphy->fw_version) == 0) {
+                snprintf(ar->hw->wiphy->fw_version,
+                         sizeof(ar->hw->wiphy->fw_version),
+                         "%u.%u",
+                         ar->fw_version_major,
+                         ar->fw_version_minor);
+        }
+
+        num_mem_reqs = __le32_to_cpu(ev->num_mem_reqs);
+
+        if (num_mem_reqs > ATH10K_MAX_MEM_REQS) {
+                ath10k_warn("requested memory chunks number (%d) exceeds the limit\n",
+                            num_mem_reqs);
+                return;
+        }
+
+        if (!num_mem_reqs)
+                goto exit;
+
+        ath10k_dbg(ATH10K_DBG_WMI, "firmware has requested %d memory chunks\n",
+                   num_mem_reqs);
+
+        for (i = 0; i < num_mem_reqs; ++i) {
+                req_id = __le32_to_cpu(ev->mem_reqs[i].req_id);
+                num_units = __le32_to_cpu(ev->mem_reqs[i].num_units);
+                unit_size = __le32_to_cpu(ev->mem_reqs[i].unit_size);
+                num_unit_info = __le32_to_cpu(ev->mem_reqs[i].num_unit_info);
+
+                if (num_unit_info & NUM_UNITS_IS_NUM_PEERS)
+                        /* number of units to allocate is number of
+                         * peers, 1 extra for self peer on target */
+                        /* this needs to be tied, host and target
+                         * can get out of sync */
+                        num_units = TARGET_10X_NUM_PEERS + 1;
+                else if (num_unit_info & NUM_UNITS_IS_NUM_VDEVS)
+                        num_units = TARGET_10X_NUM_VDEVS + 1;
+
+                ath10k_dbg(ATH10K_DBG_WMI,
+                           "wmi mem_req_id %d num_units %d num_unit_info %d unit size %d actual units %d\n",
+                           req_id,
+                           __le32_to_cpu(ev->mem_reqs[i].num_units),
+                           num_unit_info,
+                           unit_size,
+                           num_units);
+
+                ret = ath10k_wmi_alloc_host_mem(ar, req_id, num_units,
+                                                unit_size);
+                if (ret)
+                        return;
+        }
+
+exit:
+        ath10k_dbg(ATH10K_DBG_WMI,
+                   "wmi event service ready sw_ver 0x%08x abi_ver %u phy_cap 0x%08x ht_cap 0x%08x vht_cap 0x%08x vht_supp_msc 0x%08x sys_cap_info 0x%08x mem_reqs %u num_rf_chains %u\n",
+                   __le32_to_cpu(ev->sw_version),
+                   __le32_to_cpu(ev->abi_version),
+                   __le32_to_cpu(ev->phy_capability),
+                   __le32_to_cpu(ev->ht_cap_info),
+                   __le32_to_cpu(ev->vht_cap_info),
+                   __le32_to_cpu(ev->vht_supp_mcs),
+                   __le32_to_cpu(ev->sys_cap_info),
+                   __le32_to_cpu(ev->num_mem_reqs),
+                   __le32_to_cpu(ev->num_rf_chains));
+
+        complete(&ar->wmi.service_ready);
+}
+
 static int ath10k_wmi_ready_event_rx(struct ath10k *ar, struct sk_buff *skb)
 {
         struct wmi_ready_event *ev = (struct wmi_ready_event *)skb->data;
@@ -1055,7 +1731,7 @@ static int ath10k_wmi_ready_event_rx(struct ath10k *ar, struct sk_buff *skb)
         return 0;
 }
 
-static void ath10k_wmi_process_rx(struct ath10k *ar, struct sk_buff *skb)
+static void ath10k_wmi_main_process_rx(struct ath10k *ar, struct sk_buff *skb)
 {
         struct wmi_cmd_hdr *cmd_hdr;
         enum wmi_event_id id;
@@ -1174,9 +1850,138 @@ static void ath10k_wmi_process_rx(struct ath10k *ar, struct sk_buff *skb)
         dev_kfree_skb(skb);
 }
 
+static void ath10k_wmi_10x_process_rx(struct ath10k *ar, struct sk_buff *skb)
+{
+        struct wmi_cmd_hdr *cmd_hdr;
+        enum wmi_10x_event_id id;
+        u16 len;
+
+        cmd_hdr = (struct wmi_cmd_hdr *)skb->data;
+        id = MS(__le32_to_cpu(cmd_hdr->cmd_id), WMI_CMD_HDR_CMD_ID);
+
+        if (skb_pull(skb, sizeof(struct wmi_cmd_hdr)) == NULL)
+                return;
+
+        len = skb->len;
+
+        trace_ath10k_wmi_event(id, skb->data, skb->len);
+
+        switch (id) {
+        case WMI_10X_MGMT_RX_EVENTID:
+                ath10k_wmi_event_mgmt_rx(ar, skb);
+                /* mgmt_rx() owns the skb now! */
+                return;
+        case WMI_10X_SCAN_EVENTID:
+                ath10k_wmi_event_scan(ar, skb);
+                break;
+        case WMI_10X_CHAN_INFO_EVENTID:
+                ath10k_wmi_event_chan_info(ar, skb);
+                break;
+        case WMI_10X_ECHO_EVENTID:
+                ath10k_wmi_event_echo(ar, skb);
+                break;
+        case WMI_10X_DEBUG_MESG_EVENTID:
+                ath10k_wmi_event_debug_mesg(ar, skb);
+                break;
+        case WMI_10X_UPDATE_STATS_EVENTID:
+                ath10k_wmi_event_update_stats(ar, skb);
+                break;
+        case WMI_10X_VDEV_START_RESP_EVENTID:
+                ath10k_wmi_event_vdev_start_resp(ar, skb);
+                break;
+        case WMI_10X_VDEV_STOPPED_EVENTID:
+                ath10k_wmi_event_vdev_stopped(ar, skb);
+                break;
+        case WMI_10X_PEER_STA_KICKOUT_EVENTID:
+                ath10k_wmi_event_peer_sta_kickout(ar, skb);
+                break;
+        case WMI_10X_HOST_SWBA_EVENTID:
+                ath10k_wmi_event_host_swba(ar, skb);
+                break;
+        case WMI_10X_TBTTOFFSET_UPDATE_EVENTID:
+                ath10k_wmi_event_tbttoffset_update(ar, skb);
+                break;
+        case WMI_10X_PHYERR_EVENTID:
+                ath10k_wmi_event_phyerr(ar, skb);
+                break;
+        case WMI_10X_ROAM_EVENTID:
+                ath10k_wmi_event_roam(ar, skb);
+                break;
+        case WMI_10X_PROFILE_MATCH:
+                ath10k_wmi_event_profile_match(ar, skb);
+                break;
+        case WMI_10X_DEBUG_PRINT_EVENTID:
+                ath10k_wmi_event_debug_print(ar, skb);
+                break;
+        case WMI_10X_PDEV_QVIT_EVENTID:
+                ath10k_wmi_event_pdev_qvit(ar, skb);
+                break;
+        case WMI_10X_WLAN_PROFILE_DATA_EVENTID:
+                ath10k_wmi_event_wlan_profile_data(ar, skb);
+                break;
+        case WMI_10X_RTT_MEASUREMENT_REPORT_EVENTID:
+                ath10k_wmi_event_rtt_measurement_report(ar, skb);
+                break;
+        case WMI_10X_TSF_MEASUREMENT_REPORT_EVENTID:
+                ath10k_wmi_event_tsf_measurement_report(ar, skb);
+                break;
+        case WMI_10X_RTT_ERROR_REPORT_EVENTID:
+                ath10k_wmi_event_rtt_error_report(ar, skb);
+                break;
+        case WMI_10X_WOW_WAKEUP_HOST_EVENTID:
+                ath10k_wmi_event_wow_wakeup_host(ar, skb);
+                break;
+        case WMI_10X_DCS_INTERFERENCE_EVENTID:
+                ath10k_wmi_event_dcs_interference(ar, skb);
+                break;
+        case WMI_10X_PDEV_TPC_CONFIG_EVENTID:
+                ath10k_wmi_event_pdev_tpc_config(ar, skb);
+                break;
+        case WMI_10X_INST_RSSI_STATS_EVENTID:
+                ath10k_wmi_event_inst_rssi_stats(ar, skb);
+                break;
+        case WMI_10X_VDEV_STANDBY_REQ_EVENTID:
+                ath10k_wmi_event_vdev_standby_req(ar, skb);
+                break;
+        case WMI_10X_VDEV_RESUME_REQ_EVENTID:
+                ath10k_wmi_event_vdev_resume_req(ar, skb);
+                break;
+        case WMI_10X_SERVICE_READY_EVENTID:
+                ath10k_wmi_10x_service_ready_event_rx(ar, skb);
+                break;
+        case WMI_10X_READY_EVENTID:
+                ath10k_wmi_ready_event_rx(ar, skb);
+                break;
+        default:
+                ath10k_warn("Unknown eventid: %d\n", id);
+                break;
+        }
+
+        dev_kfree_skb(skb);
+}
+
+
+static void ath10k_wmi_process_rx(struct ath10k *ar, struct sk_buff *skb)
+{
+        if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features))
+                ath10k_wmi_10x_process_rx(ar, skb);
+        else
+                ath10k_wmi_main_process_rx(ar, skb);
+}
+
 /* WMI Initialization functions */
 int ath10k_wmi_attach(struct ath10k *ar)
 {
+        if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
+                ar->wmi.cmd = &wmi_10x_cmd_map;
+                ar->wmi.vdev_param = &wmi_10x_vdev_param_map;
+                ar->wmi.pdev_param = &wmi_10x_pdev_param_map;
+        } else {
+                ar->wmi.cmd = &wmi_cmd_map;
+                ar->wmi.vdev_param = &wmi_vdev_param_map;
+                ar->wmi.pdev_param = &wmi_pdev_param_map;
+        }
+
         init_completion(&ar->wmi.service_ready);
         init_completion(&ar->wmi.unified_ready);
         init_waitqueue_head(&ar->wmi.tx_credits_wq);
@@ -1186,6 +1991,17 @@ int ath10k_wmi_attach(struct ath10k *ar)
 
 void ath10k_wmi_detach(struct ath10k *ar)
 {
+        int i;
+
+        /* free the host memory chunks requested by firmware */
+        for (i = 0; i < ar->wmi.num_mem_chunks; i++) {
+                dma_free_coherent(ar->dev,
+                                  ar->wmi.mem_chunks[i].len,
+                                  ar->wmi.mem_chunks[i].vaddr,
+                                  ar->wmi.mem_chunks[i].paddr);
+        }
+
+        ar->wmi.num_mem_chunks = 0;
 }
 
 int ath10k_wmi_connect_htc_service(struct ath10k *ar)
@@ -1237,7 +2053,8 @@ int ath10k_wmi_pdev_set_regdomain(struct ath10k *ar, u16 rd, u16 rd2g,
                    "wmi pdev regdomain rd %x rd2g %x rd5g %x ctl2g %x ctl5g %x\n",
                    rd, rd2g, rd5g, ctl2g, ctl5g);
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_PDEV_SET_REGDOMAIN_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb,
+                                   ar->wmi.cmd->pdev_set_regdomain_cmdid);
 }
 
 int ath10k_wmi_pdev_set_channel(struct ath10k *ar,
@@ -1267,7 +2084,8 @@ int ath10k_wmi_pdev_set_channel(struct ath10k *ar,
                    "wmi set channel mode %d freq %d\n",
                    arg->mode, arg->freq);
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_PDEV_SET_CHANNEL_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb,
+                                   ar->wmi.cmd->pdev_set_channel_cmdid);
 }
 
 int ath10k_wmi_pdev_suspend_target(struct ath10k *ar)
@@ -1282,7 +2100,7 @@ int ath10k_wmi_pdev_suspend_target(struct ath10k *ar)
         cmd = (struct wmi_pdev_suspend_cmd *)skb->data;
         cmd->suspend_opt = WMI_PDEV_SUSPEND;
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_PDEV_SUSPEND_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->pdev_suspend_cmdid);
 }
 
 int ath10k_wmi_pdev_resume_target(struct ath10k *ar)
@@ -1293,15 +2111,19 @@ int ath10k_wmi_pdev_resume_target(struct ath10k *ar)
         if (skb == NULL)
                 return -ENOMEM;
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_PDEV_RESUME_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->pdev_resume_cmdid);
 }
 
-int ath10k_wmi_pdev_set_param(struct ath10k *ar, enum wmi_pdev_param id,
-                              u32 value)
+int ath10k_wmi_pdev_set_param(struct ath10k *ar, u32 id, u32 value)
 {
         struct wmi_pdev_set_param_cmd *cmd;
         struct sk_buff *skb;
 
+        if (id == WMI_PDEV_PARAM_UNSUPPORTED) {
+                ath10k_warn("pdev param %d not supported by firmware\n", id);
+                return -EOPNOTSUPP;
+        }
+
         skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
         if (!skb)
                 return -ENOMEM;
@@ -1312,15 +2134,16 @@ int ath10k_wmi_pdev_set_param(struct ath10k *ar, enum wmi_pdev_param id,
 
         ath10k_dbg(ATH10K_DBG_WMI, "wmi pdev set param %d value %d\n",
                    id, value);
-        return ath10k_wmi_cmd_send(ar, skb, WMI_PDEV_SET_PARAM_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->pdev_set_param_cmdid);
 }
 
-int ath10k_wmi_cmd_init(struct ath10k *ar)
+static int ath10k_wmi_main_cmd_init(struct ath10k *ar)
 {
         struct wmi_init_cmd *cmd;
         struct sk_buff *buf;
         struct wmi_resource_config config = {};
-        u32 val;
+        u32 len, val;
+        int i;
 
         config.num_vdevs = __cpu_to_le32(TARGET_NUM_VDEVS);
         config.num_peers = __cpu_to_le32(TARGET_NUM_PEERS + TARGET_NUM_VDEVS);
@@ -1373,23 +2196,158 @@ int ath10k_wmi_cmd_init(struct ath10k *ar)
         config.num_msdu_desc = __cpu_to_le32(TARGET_NUM_MSDU_DESC);
         config.max_frag_entries = __cpu_to_le32(TARGET_MAX_FRAG_ENTRIES);
 
-        buf = ath10k_wmi_alloc_skb(sizeof(*cmd));
+        len = sizeof(*cmd) +
+              (sizeof(struct host_memory_chunk) * ar->wmi.num_mem_chunks);
+
+        buf = ath10k_wmi_alloc_skb(len);
         if (!buf)
                 return -ENOMEM;
 
         cmd = (struct wmi_init_cmd *)buf->data;
-        cmd->num_host_mem_chunks = 0;
+
+        if (ar->wmi.num_mem_chunks == 0) {
+                cmd->num_host_mem_chunks = 0;
+                goto out;
+        }
+
+        ath10k_dbg(ATH10K_DBG_WMI, "wmi sending %d memory chunks info.\n",
+                   __cpu_to_le32(ar->wmi.num_mem_chunks));
+
+        cmd->num_host_mem_chunks = __cpu_to_le32(ar->wmi.num_mem_chunks);
+
+        for (i = 0; i < ar->wmi.num_mem_chunks; i++) {
+                cmd->host_mem_chunks[i].ptr =
+                        __cpu_to_le32(ar->wmi.mem_chunks[i].paddr);
+                cmd->host_mem_chunks[i].size =
+                        __cpu_to_le32(ar->wmi.mem_chunks[i].len);
+                cmd->host_mem_chunks[i].req_id =
+                        __cpu_to_le32(ar->wmi.mem_chunks[i].req_id);
+
+                ath10k_dbg(ATH10K_DBG_WMI,
+                           "wmi chunk %d len %d requested, addr 0x%x\n",
+                           i,
+                           cmd->host_mem_chunks[i].size,
+                           cmd->host_mem_chunks[i].ptr);
+        }
+out:
         memcpy(&cmd->resource_config, &config, sizeof(config));
 
         ath10k_dbg(ATH10K_DBG_WMI, "wmi init\n");
-        return ath10k_wmi_cmd_send(ar, buf, WMI_INIT_CMDID);
+        return ath10k_wmi_cmd_send(ar, buf, ar->wmi.cmd->init_cmdid);
+}
+
+static int ath10k_wmi_10x_cmd_init(struct ath10k *ar)
+{
+        struct wmi_init_cmd_10x *cmd;
+        struct sk_buff *buf;
+        struct wmi_resource_config_10x config = {};
+        u32 len, val;
+        int i;
+
+        config.num_vdevs = __cpu_to_le32(TARGET_10X_NUM_VDEVS);
+        config.num_peers = __cpu_to_le32(TARGET_10X_NUM_PEERS);
+        config.num_peer_keys = __cpu_to_le32(TARGET_10X_NUM_PEER_KEYS);
+        config.num_tids = __cpu_to_le32(TARGET_10X_NUM_TIDS);
+        config.ast_skid_limit = __cpu_to_le32(TARGET_10X_AST_SKID_LIMIT);
+        config.tx_chain_mask = __cpu_to_le32(TARGET_10X_TX_CHAIN_MASK);
+        config.rx_chain_mask = __cpu_to_le32(TARGET_10X_RX_CHAIN_MASK);
+        config.rx_timeout_pri_vo = __cpu_to_le32(TARGET_10X_RX_TIMEOUT_LO_PRI);
+        config.rx_timeout_pri_vi = __cpu_to_le32(TARGET_10X_RX_TIMEOUT_LO_PRI);
+        config.rx_timeout_pri_be = __cpu_to_le32(TARGET_10X_RX_TIMEOUT_LO_PRI);
+        config.rx_timeout_pri_bk = __cpu_to_le32(TARGET_10X_RX_TIMEOUT_HI_PRI);
+        config.rx_decap_mode = __cpu_to_le32(TARGET_10X_RX_DECAP_MODE);
+
+        config.scan_max_pending_reqs =
+                __cpu_to_le32(TARGET_10X_SCAN_MAX_PENDING_REQS);
+
+        config.bmiss_offload_max_vdev =
+                __cpu_to_le32(TARGET_10X_BMISS_OFFLOAD_MAX_VDEV);
+
+        config.roam_offload_max_vdev =
+                __cpu_to_le32(TARGET_10X_ROAM_OFFLOAD_MAX_VDEV);
+
+        config.roam_offload_max_ap_profiles =
+                __cpu_to_le32(TARGET_10X_ROAM_OFFLOAD_MAX_AP_PROFILES);
+
+        config.num_mcast_groups = __cpu_to_le32(TARGET_10X_NUM_MCAST_GROUPS);
+        config.num_mcast_table_elems =
+                __cpu_to_le32(TARGET_10X_NUM_MCAST_TABLE_ELEMS);
+
+        config.mcast2ucast_mode = __cpu_to_le32(TARGET_10X_MCAST2UCAST_MODE);
+        config.tx_dbg_log_size = __cpu_to_le32(TARGET_10X_TX_DBG_LOG_SIZE);
+        config.num_wds_entries = __cpu_to_le32(TARGET_10X_NUM_WDS_ENTRIES);
+        config.dma_burst_size = __cpu_to_le32(TARGET_10X_DMA_BURST_SIZE);
+        config.mac_aggr_delim = __cpu_to_le32(TARGET_10X_MAC_AGGR_DELIM);
+
+        val = TARGET_10X_RX_SKIP_DEFRAG_TIMEOUT_DUP_DETECTION_CHECK;
+        config.rx_skip_defrag_timeout_dup_detection_check = __cpu_to_le32(val);
+
+        config.vow_config = __cpu_to_le32(TARGET_10X_VOW_CONFIG);
+
+        config.num_msdu_desc = __cpu_to_le32(TARGET_10X_NUM_MSDU_DESC);
+        config.max_frag_entries = __cpu_to_le32(TARGET_10X_MAX_FRAG_ENTRIES);
+
+        len = sizeof(*cmd) +
+              (sizeof(struct host_memory_chunk) * ar->wmi.num_mem_chunks);
+
+        buf = ath10k_wmi_alloc_skb(len);
+        if (!buf)
+                return -ENOMEM;
+
+        cmd = (struct wmi_init_cmd_10x *)buf->data;
+
+        if (ar->wmi.num_mem_chunks == 0) {
+                cmd->num_host_mem_chunks = 0;
+                goto out;
+        }
+
+        ath10k_dbg(ATH10K_DBG_WMI, "wmi sending %d memory chunks info.\n",
+                   __cpu_to_le32(ar->wmi.num_mem_chunks));
+
+        cmd->num_host_mem_chunks = __cpu_to_le32(ar->wmi.num_mem_chunks);
+
+        for (i = 0; i < ar->wmi.num_mem_chunks; i++) {
+                cmd->host_mem_chunks[i].ptr =
+                        __cpu_to_le32(ar->wmi.mem_chunks[i].paddr);
+                cmd->host_mem_chunks[i].size =
+                        __cpu_to_le32(ar->wmi.mem_chunks[i].len);
+                cmd->host_mem_chunks[i].req_id =
+                        __cpu_to_le32(ar->wmi.mem_chunks[i].req_id);
+
+                ath10k_dbg(ATH10K_DBG_WMI,
+                           "wmi chunk %d len %d requested, addr 0x%x\n",
+                           i,
+                           cmd->host_mem_chunks[i].size,
+                           cmd->host_mem_chunks[i].ptr);
+        }
+out:
+        memcpy(&cmd->resource_config, &config, sizeof(config));
+
+        ath10k_dbg(ATH10K_DBG_WMI, "wmi init 10x\n");
+        return ath10k_wmi_cmd_send(ar, buf, ar->wmi.cmd->init_cmdid);
+}
+
+int ath10k_wmi_cmd_init(struct ath10k *ar)
+{
+        int ret;
+
+        if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features))
+                ret = ath10k_wmi_10x_cmd_init(ar);
+        else
+                ret = ath10k_wmi_main_cmd_init(ar);
+
+        return ret;
 }
 
-static int ath10k_wmi_start_scan_calc_len(const struct wmi_start_scan_arg *arg)
+static int ath10k_wmi_start_scan_calc_len(struct ath10k *ar,
+                                          const struct wmi_start_scan_arg *arg)
 {
         int len;
 
-        len = sizeof(struct wmi_start_scan_cmd);
+        if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features))
+                len = sizeof(struct wmi_start_scan_cmd_10x);
+        else
+                len = sizeof(struct wmi_start_scan_cmd);
 
         if (arg->ie_len) {
                 if (!arg->ie)
@@ -1449,7 +2407,7 @@ int ath10k_wmi_start_scan(struct ath10k *ar,
         int len = 0;
         int i;
 
-        len = ath10k_wmi_start_scan_calc_len(arg);
+        len = ath10k_wmi_start_scan_calc_len(ar, arg);
         if (len < 0)
                 return len; /* len contains error code here */
 
@@ -1481,7 +2439,14 @@ int ath10k_wmi_start_scan(struct ath10k *ar,
         cmd->scan_ctrl_flags    = __cpu_to_le32(arg->scan_ctrl_flags);
 
         /* TLV list starts after fields included in the struct */
-        off = sizeof(*cmd);
+        /* There's just one filed that differes the two start_scan
+         * structures - burst_duration, which we are not using btw,
+           no point to make the split here, just shift the buffer to fit with
+           given FW */
+        if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features))
+                off = sizeof(struct wmi_start_scan_cmd_10x);
+        else
+                off = sizeof(struct wmi_start_scan_cmd);
 
         if (arg->n_channels) {
                 channels = (void *)skb->data + off;
@@ -1543,7 +2508,7 @@ int ath10k_wmi_start_scan(struct ath10k *ar,
         }
 
         ath10k_dbg(ATH10K_DBG_WMI, "wmi start scan\n");
-        return ath10k_wmi_cmd_send(ar, skb, WMI_START_SCAN_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->start_scan_cmdid);
 }
 
 void ath10k_wmi_start_scan_init(struct ath10k *ar,
@@ -1559,7 +2524,7 @@ void ath10k_wmi_start_scan_init(struct ath10k *ar,
         arg->repeat_probe_time = 0;
         arg->probe_spacing_time = 0;
         arg->idle_time = 0;
-        arg->max_scan_time = 5000;
+        arg->max_scan_time = 20000;
         arg->probe_delay = 5;
         arg->notify_scan_events = WMI_SCAN_EVENT_STARTED
                 | WMI_SCAN_EVENT_COMPLETED
@@ -1603,7 +2568,7 @@ int ath10k_wmi_stop_scan(struct ath10k *ar, const struct wmi_stop_scan_arg *arg)
         ath10k_dbg(ATH10K_DBG_WMI,
                    "wmi stop scan reqid %d req_type %d vdev/scan_id %d\n",
                    arg->req_id, arg->req_type, arg->u.scan_id);
-        return ath10k_wmi_cmd_send(ar, skb, WMI_STOP_SCAN_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->stop_scan_cmdid);
 }
 
 int ath10k_wmi_vdev_create(struct ath10k *ar, u32 vdev_id,
@@ -1628,7 +2593,7 @@ int ath10k_wmi_vdev_create(struct ath10k *ar, u32 vdev_id,
                    "WMI vdev create: id %d type %d subtype %d macaddr %pM\n",
                    vdev_id, type, subtype, macaddr);
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_VDEV_CREATE_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_create_cmdid);
 }
 
 int ath10k_wmi_vdev_delete(struct ath10k *ar, u32 vdev_id)
@@ -1646,20 +2611,20 @@ int ath10k_wmi_vdev_delete(struct ath10k *ar, u32 vdev_id)
         ath10k_dbg(ATH10K_DBG_WMI,
                    "WMI vdev delete id %d\n", vdev_id);
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_VDEV_DELETE_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_delete_cmdid);
 }
 
 static int ath10k_wmi_vdev_start_restart(struct ath10k *ar,
                                 const struct wmi_vdev_start_request_arg *arg,
-                                enum wmi_cmd_id cmd_id)
+                                u32 cmd_id)
 {
         struct wmi_vdev_start_request_cmd *cmd;
         struct sk_buff *skb;
         const char *cmdname;
         u32 flags = 0;
 
-        if (cmd_id != WMI_VDEV_START_REQUEST_CMDID &&
-            cmd_id != WMI_VDEV_RESTART_REQUEST_CMDID)
+        if (cmd_id != ar->wmi.cmd->vdev_start_request_cmdid &&
+            cmd_id != ar->wmi.cmd->vdev_restart_request_cmdid)
                 return -EINVAL;
         if (WARN_ON(arg->ssid && arg->ssid_len == 0))
                 return -EINVAL;
@@ -1668,9 +2633,9 @@ static int ath10k_wmi_vdev_start_restart(struct ath10k *ar,
         if (WARN_ON(arg->ssid_len > sizeof(cmd->ssid.ssid)))
                 return -EINVAL;
 
-        if (cmd_id == WMI_VDEV_START_REQUEST_CMDID)
+        if (cmd_id == ar->wmi.cmd->vdev_start_request_cmdid)
                 cmdname = "start";
-        else if (cmd_id == WMI_VDEV_RESTART_REQUEST_CMDID)
+        else if (cmd_id == ar->wmi.cmd->vdev_restart_request_cmdid)
                 cmdname = "restart";
         else
                 return -EINVAL; /* should not happen, we already check cmd_id */
@@ -1721,15 +2686,17 @@ static int ath10k_wmi_vdev_start_restart(struct ath10k *ar,
 int ath10k_wmi_vdev_start(struct ath10k *ar,
                           const struct wmi_vdev_start_request_arg *arg)
 {
-        return ath10k_wmi_vdev_start_restart(ar, arg,
-                                             WMI_VDEV_START_REQUEST_CMDID);
+        u32 cmd_id = ar->wmi.cmd->vdev_start_request_cmdid;
+
+        return ath10k_wmi_vdev_start_restart(ar, arg, cmd_id);
 }
 
 int ath10k_wmi_vdev_restart(struct ath10k *ar,
                      const struct wmi_vdev_start_request_arg *arg)
 {
-        return ath10k_wmi_vdev_start_restart(ar, arg,
-                                             WMI_VDEV_RESTART_REQUEST_CMDID);
+        u32 cmd_id = ar->wmi.cmd->vdev_restart_request_cmdid;
+
+        return ath10k_wmi_vdev_start_restart(ar, arg, cmd_id);
 }
 
 int ath10k_wmi_vdev_stop(struct ath10k *ar, u32 vdev_id)
@@ -1746,7 +2713,7 @@ int ath10k_wmi_vdev_stop(struct ath10k *ar, u32 vdev_id)
 
         ath10k_dbg(ATH10K_DBG_WMI, "wmi vdev stop id 0x%x\n", vdev_id);
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_VDEV_STOP_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_stop_cmdid);
 }
 
 int ath10k_wmi_vdev_up(struct ath10k *ar, u32 vdev_id, u32 aid, const u8 *bssid)
@@ -1767,7 +2734,7 @@ int ath10k_wmi_vdev_up(struct ath10k *ar, u32 vdev_id, u32 aid, const u8 *bssid)
                    "wmi mgmt vdev up id 0x%x assoc id %d bssid %pM\n",
                    vdev_id, aid, bssid);
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_VDEV_UP_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_up_cmdid);
 }
 
 int ath10k_wmi_vdev_down(struct ath10k *ar, u32 vdev_id)
@@ -1785,15 +2752,22 @@ int ath10k_wmi_vdev_down(struct ath10k *ar, u32 vdev_id)
         ath10k_dbg(ATH10K_DBG_WMI,
                    "wmi mgmt vdev down id 0x%x\n", vdev_id);
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_VDEV_DOWN_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_down_cmdid);
 }
 
 int ath10k_wmi_vdev_set_param(struct ath10k *ar, u32 vdev_id,
-                              enum wmi_vdev_param param_id, u32 param_value)
+                              u32 param_id, u32 param_value)
 {
         struct wmi_vdev_set_param_cmd *cmd;
         struct sk_buff *skb;
 
+        if (param_id == WMI_VDEV_PARAM_UNSUPPORTED) {
+                ath10k_dbg(ATH10K_DBG_WMI,
+                           "vdev param %d not supported by firmware\n",
+                            param_id);
+                return -EOPNOTSUPP;
+        }
+
         skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
         if (!skb)
                 return -ENOMEM;
@@ -1807,7 +2781,7 @@ int ath10k_wmi_vdev_set_param(struct ath10k *ar, u32 vdev_id,
                    "wmi vdev id 0x%x set param %d value %d\n",
                    vdev_id, param_id, param_value);
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_VDEV_SET_PARAM_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_set_param_cmdid);
 }
 
 int ath10k_wmi_vdev_install_key(struct ath10k *ar,
@@ -1842,7 +2816,8 @@ int ath10k_wmi_vdev_install_key(struct ath10k *ar,
         ath10k_dbg(ATH10K_DBG_WMI,
                    "wmi vdev install key idx %d cipher %d len %d\n",
                    arg->key_idx, arg->key_cipher, arg->key_len);
-        return ath10k_wmi_cmd_send(ar, skb, WMI_VDEV_INSTALL_KEY_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb,
+                                   ar->wmi.cmd->vdev_install_key_cmdid);
 }
 
 int ath10k_wmi_peer_create(struct ath10k *ar, u32 vdev_id,
@@ -1862,7 +2837,7 @@ int ath10k_wmi_peer_create(struct ath10k *ar, u32 vdev_id,
         ath10k_dbg(ATH10K_DBG_WMI,
                    "wmi peer create vdev_id %d peer_addr %pM\n",
                    vdev_id, peer_addr);
-        return ath10k_wmi_cmd_send(ar, skb, WMI_PEER_CREATE_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_create_cmdid);
 }
 
 int ath10k_wmi_peer_delete(struct ath10k *ar, u32 vdev_id,
@@ -1882,7 +2857,7 @@ int ath10k_wmi_peer_delete(struct ath10k *ar, u32 vdev_id,
         ath10k_dbg(ATH10K_DBG_WMI,
                    "wmi peer delete vdev_id %d peer_addr %pM\n",
                    vdev_id, peer_addr);
-        return ath10k_wmi_cmd_send(ar, skb, WMI_PEER_DELETE_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_delete_cmdid);
 }
 
 int ath10k_wmi_peer_flush(struct ath10k *ar, u32 vdev_id,
@@ -1903,7 +2878,7 @@ int ath10k_wmi_peer_flush(struct ath10k *ar, u32 vdev_id,
         ath10k_dbg(ATH10K_DBG_WMI,
                    "wmi peer flush vdev_id %d peer_addr %pM tids %08x\n",
                    vdev_id, peer_addr, tid_bitmap);
-        return ath10k_wmi_cmd_send(ar, skb, WMI_PEER_FLUSH_TIDS_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_flush_tids_cmdid);
 }
 
 int ath10k_wmi_peer_set_param(struct ath10k *ar, u32 vdev_id,
@@ -1927,7 +2902,7 @@ int ath10k_wmi_peer_set_param(struct ath10k *ar, u32 vdev_id,
                    "wmi vdev %d peer 0x%pM set param %d value %d\n",
                    vdev_id, peer_addr, param_id, param_value);
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_PEER_SET_PARAM_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_set_param_cmdid);
 }
 
 int ath10k_wmi_set_psmode(struct ath10k *ar, u32 vdev_id,
@@ -1948,7 +2923,8 @@ int ath10k_wmi_set_psmode(struct ath10k *ar, u32 vdev_id,
                    "wmi set powersave id 0x%x mode %d\n",
                    vdev_id, psmode);
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_STA_POWERSAVE_MODE_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb,
+                                   ar->wmi.cmd->sta_powersave_mode_cmdid);
 }
 
 int ath10k_wmi_set_sta_ps_param(struct ath10k *ar, u32 vdev_id,
@@ -1970,7 +2946,8 @@ int ath10k_wmi_set_sta_ps_param(struct ath10k *ar, u32 vdev_id,
         ath10k_dbg(ATH10K_DBG_WMI,
                    "wmi sta ps param vdev_id 0x%x param %d value %d\n",
                    vdev_id, param_id, value);
-        return ath10k_wmi_cmd_send(ar, skb, WMI_STA_POWERSAVE_PARAM_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb,
+                                   ar->wmi.cmd->sta_powersave_param_cmdid);
 }
 
 int ath10k_wmi_set_ap_ps_param(struct ath10k *ar, u32 vdev_id, const u8 *mac,
@@ -1996,7 +2973,8 @@ int ath10k_wmi_set_ap_ps_param(struct ath10k *ar, u32 vdev_id, const u8 *mac,
                    "wmi ap ps param vdev_id 0x%X param %d value %d mac_addr %pM\n",
                    vdev_id, param_id, value, mac);
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_AP_PS_PEER_PARAM_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb,
+                                   ar->wmi.cmd->ap_ps_peer_param_cmdid);
 }
 
 int ath10k_wmi_scan_chan_list(struct ath10k *ar,
@@ -2049,7 +3027,7 @@ int ath10k_wmi_scan_chan_list(struct ath10k *ar,
                 ci->flags            |= __cpu_to_le32(flags);
         }
 
-        return ath10k_wmi_cmd_send(ar, skb, WMI_SCAN_CHAN_LIST_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->scan_chan_list_cmdid);
 }
 
 int ath10k_wmi_peer_assoc(struct ath10k *ar,
@@ -2108,7 +3086,7 @@ int ath10k_wmi_peer_assoc(struct ath10k *ar,
         ath10k_dbg(ATH10K_DBG_WMI,
                    "wmi peer assoc vdev %d addr %pM\n",
                    arg->vdev_id, arg->addr);
-        return ath10k_wmi_cmd_send(ar, skb, WMI_PEER_ASSOC_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_assoc_cmdid);
 }
 
 int ath10k_wmi_beacon_send_nowait(struct ath10k *ar,
@@ -2128,7 +3106,7 @@ int ath10k_wmi_beacon_send_nowait(struct ath10k *ar,
         cmd->hdr.bcn_len  = __cpu_to_le32(arg->bcn_len);
         memcpy(cmd->bcn, arg->bcn, arg->bcn_len);
 
-        return ath10k_wmi_cmd_send_nowait(ar, skb, WMI_BCN_TX_CMDID);
+        return ath10k_wmi_cmd_send_nowait(ar, skb, ar->wmi.cmd->bcn_tx_cmdid);
 }
 
 static void ath10k_wmi_pdev_set_wmm_param(struct wmi_wmm_params *params,
@@ -2159,7 +3137,8 @@ int ath10k_wmi_pdev_set_wmm_params(struct ath10k *ar,
         ath10k_wmi_pdev_set_wmm_param(&cmd->ac_vo, &arg->ac_vo);
 
         ath10k_dbg(ATH10K_DBG_WMI, "wmi pdev set wmm params\n");
-        return ath10k_wmi_cmd_send(ar, skb, WMI_PDEV_SET_WMM_PARAMS_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb,
+                                   ar->wmi.cmd->pdev_set_wmm_params_cmdid);
 }
 
 int ath10k_wmi_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id)
@@ -2175,7 +3154,7 @@ int ath10k_wmi_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id)
         cmd->stats_id = __cpu_to_le32(stats_id);
 
         ath10k_dbg(ATH10K_DBG_WMI, "wmi request stats %d\n", (int)stats_id);
-        return ath10k_wmi_cmd_send(ar, skb, WMI_REQUEST_STATS_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->request_stats_cmdid);
 }
 
 int ath10k_wmi_force_fw_hang(struct ath10k *ar,
@@ -2194,5 +3173,5 @@ int ath10k_wmi_force_fw_hang(struct ath10k *ar,
 
         ath10k_dbg(ATH10K_DBG_WMI, "wmi force fw hang %d delay %d\n",
                    type, delay_ms);
-        return ath10k_wmi_cmd_send(ar, skb, WMI_FORCE_FW_HANG_CMDID);
+        return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->force_fw_hang_cmdid);
 }
diff --git a/drivers/net/wireless/ath/ath10k/wmi.h b/drivers/net/wireless/ath/ath10k/wmi.h
index 2c52c23107dd..78c991aec7f9 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.h
+++ b/drivers/net/wireless/ath/ath10k/wmi.h
@@ -208,6 +208,118 @@ struct wmi_mac_addr {
         (c_macaddr)[5] = (((pwmi_mac_addr)->word1) >> 8) & 0xff; \
         } while (0)
 
+struct wmi_cmd_map {
+        u32 init_cmdid;
+        u32 start_scan_cmdid;
+        u32 stop_scan_cmdid;
+        u32 scan_chan_list_cmdid;
+        u32 scan_sch_prio_tbl_cmdid;
+        u32 pdev_set_regdomain_cmdid;
+        u32 pdev_set_channel_cmdid;
+        u32 pdev_set_param_cmdid;
+        u32 pdev_pktlog_enable_cmdid;
+        u32 pdev_pktlog_disable_cmdid;
+        u32 pdev_set_wmm_params_cmdid;
+        u32 pdev_set_ht_cap_ie_cmdid;
+        u32 pdev_set_vht_cap_ie_cmdid;
+        u32 pdev_set_dscp_tid_map_cmdid;
+        u32 pdev_set_quiet_mode_cmdid;
+        u32 pdev_green_ap_ps_enable_cmdid;
+        u32 pdev_get_tpc_config_cmdid;
+        u32 pdev_set_base_macaddr_cmdid;
+        u32 vdev_create_cmdid;
+        u32 vdev_delete_cmdid;
+        u32 vdev_start_request_cmdid;
+        u32 vdev_restart_request_cmdid;
+        u32 vdev_up_cmdid;
+        u32 vdev_stop_cmdid;
+        u32 vdev_down_cmdid;
+        u32 vdev_set_param_cmdid;
+        u32 vdev_install_key_cmdid;
+        u32 peer_create_cmdid;
+        u32 peer_delete_cmdid;
+        u32 peer_flush_tids_cmdid;
+        u32 peer_set_param_cmdid;
+        u32 peer_assoc_cmdid;
+        u32 peer_add_wds_entry_cmdid;
+        u32 peer_remove_wds_entry_cmdid;
+        u32 peer_mcast_group_cmdid;
+        u32 bcn_tx_cmdid;
+        u32 pdev_send_bcn_cmdid;
+        u32 bcn_tmpl_cmdid;
+        u32 bcn_filter_rx_cmdid;
+        u32 prb_req_filter_rx_cmdid;
+        u32 mgmt_tx_cmdid;
+        u32 prb_tmpl_cmdid;
+        u32 addba_clear_resp_cmdid;
+        u32 addba_send_cmdid;
+        u32 addba_status_cmdid;
+        u32 delba_send_cmdid;
+        u32 addba_set_resp_cmdid;
+        u32 send_singleamsdu_cmdid;
+        u32 sta_powersave_mode_cmdid;
+        u32 sta_powersave_param_cmdid;
+        u32 sta_mimo_ps_mode_cmdid;
+        u32 pdev_dfs_enable_cmdid;
+        u32 pdev_dfs_disable_cmdid;
+        u32 roam_scan_mode;
+        u32 roam_scan_rssi_threshold;
+        u32 roam_scan_period;
+        u32 roam_scan_rssi_change_threshold;
+        u32 roam_ap_profile;
+        u32 ofl_scan_add_ap_profile;
+        u32 ofl_scan_remove_ap_profile;
+        u32 ofl_scan_period;
+        u32 p2p_dev_set_device_info;
+        u32 p2p_dev_set_discoverability;
+        u32 p2p_go_set_beacon_ie;
+        u32 p2p_go_set_probe_resp_ie;
+        u32 p2p_set_vendor_ie_data_cmdid;
+        u32 ap_ps_peer_param_cmdid;
+        u32 ap_ps_peer_uapsd_coex_cmdid;
+        u32 peer_rate_retry_sched_cmdid;
+        u32 wlan_profile_trigger_cmdid;
+        u32 wlan_profile_set_hist_intvl_cmdid;
+        u32 wlan_profile_get_profile_data_cmdid;
+        u32 wlan_profile_enable_profile_id_cmdid;
+        u32 wlan_profile_list_profile_id_cmdid;
+        u32 pdev_suspend_cmdid;
+        u32 pdev_resume_cmdid;
+        u32 add_bcn_filter_cmdid;
+        u32 rmv_bcn_filter_cmdid;
+        u32 wow_add_wake_pattern_cmdid;
+        u32 wow_del_wake_pattern_cmdid;
+        u32 wow_enable_disable_wake_event_cmdid;
+        u32 wow_enable_cmdid;
+        u32 wow_hostwakeup_from_sleep_cmdid;
+        u32 rtt_measreq_cmdid;
+        u32 rtt_tsf_cmdid;
+        u32 vdev_spectral_scan_configure_cmdid;
+        u32 vdev_spectral_scan_enable_cmdid;
+        u32 request_stats_cmdid;
+        u32 set_arp_ns_offload_cmdid;
+        u32 network_list_offload_config_cmdid;
+        u32 gtk_offload_cmdid;
+        u32 csa_offload_enable_cmdid;
+        u32 csa_offload_chanswitch_cmdid;
+        u32 chatter_set_mode_cmdid;
+        u32 peer_tid_addba_cmdid;
+        u32 peer_tid_delba_cmdid;
+        u32 sta_dtim_ps_method_cmdid;
+        u32 sta_uapsd_auto_trig_cmdid;
+        u32 sta_keepalive_cmd;
+        u32 echo_cmdid;
+        u32 pdev_utf_cmdid;
+        u32 dbglog_cfg_cmdid;
+        u32 pdev_qvit_cmdid;
+        u32 pdev_ftm_intg_cmdid;
+        u32 vdev_set_keepalive_cmdid;
+        u32 vdev_get_keepalive_cmdid;
+        u32 force_fw_hang_cmdid;
+        u32 gpio_config_cmdid;
+        u32 gpio_output_cmdid;
+};
+
 /*
  * wmi command groups.
  */
@@ -247,7 +359,9 @@ enum wmi_cmd_group {
 #define WMI_CMD_GRP(grp_id) (((grp_id) << 12) | 0x1)
 #define WMI_EVT_GRP_START_ID(grp_id) (((grp_id) << 12) | 0x1)
 
-/* Command IDs and commande events. */
+#define WMI_CMD_UNSUPPORTED 0
+
+/* Command IDs and command events for MAIN FW. */
 enum wmi_cmd_id {
         WMI_INIT_CMDID = 0x1,
 
@@ -488,6 +602,217 @@ enum wmi_event_id {
         WMI_GPIO_INPUT_EVENTID = WMI_EVT_GRP_START_ID(WMI_GRP_GPIO),
 };
 
+/* Command IDs and command events for 10.X firmware */
+enum wmi_10x_cmd_id {
+        WMI_10X_START_CMDID = 0x9000,
+        WMI_10X_END_CMDID = 0x9FFF,
+
+        /* initialize the wlan sub system */
+        WMI_10X_INIT_CMDID,
+
+        /* Scan specific commands */
+
+        WMI_10X_START_SCAN_CMDID = WMI_10X_START_CMDID,
+        WMI_10X_STOP_SCAN_CMDID,
+        WMI_10X_SCAN_CHAN_LIST_CMDID,
+        WMI_10X_ECHO_CMDID,
+
+        /* PDEV(physical device) specific commands */
+        WMI_10X_PDEV_SET_REGDOMAIN_CMDID,
+        WMI_10X_PDEV_SET_CHANNEL_CMDID,
+        WMI_10X_PDEV_SET_PARAM_CMDID,
+        WMI_10X_PDEV_PKTLOG_ENABLE_CMDID,
+        WMI_10X_PDEV_PKTLOG_DISABLE_CMDID,
+        WMI_10X_PDEV_SET_WMM_PARAMS_CMDID,
+        WMI_10X_PDEV_SET_HT_CAP_IE_CMDID,
+        WMI_10X_PDEV_SET_VHT_CAP_IE_CMDID,
+        WMI_10X_PDEV_SET_BASE_MACADDR_CMDID,
+        WMI_10X_PDEV_SET_DSCP_TID_MAP_CMDID,
+        WMI_10X_PDEV_SET_QUIET_MODE_CMDID,
+        WMI_10X_PDEV_GREEN_AP_PS_ENABLE_CMDID,
+        WMI_10X_PDEV_GET_TPC_CONFIG_CMDID,
+
+        /* VDEV(virtual device) specific commands */
+        WMI_10X_VDEV_CREATE_CMDID,
+        WMI_10X_VDEV_DELETE_CMDID,
+        WMI_10X_VDEV_START_REQUEST_CMDID,
+        WMI_10X_VDEV_RESTART_REQUEST_CMDID,
+        WMI_10X_VDEV_UP_CMDID,
+        WMI_10X_VDEV_STOP_CMDID,
+        WMI_10X_VDEV_DOWN_CMDID,
+        WMI_10X_VDEV_STANDBY_RESPONSE_CMDID,
+        WMI_10X_VDEV_RESUME_RESPONSE_CMDID,
+        WMI_10X_VDEV_SET_PARAM_CMDID,
+        WMI_10X_VDEV_INSTALL_KEY_CMDID,
+
+        /* peer specific commands */
+        WMI_10X_PEER_CREATE_CMDID,
+        WMI_10X_PEER_DELETE_CMDID,
+        WMI_10X_PEER_FLUSH_TIDS_CMDID,
+        WMI_10X_PEER_SET_PARAM_CMDID,
+        WMI_10X_PEER_ASSOC_CMDID,
+        WMI_10X_PEER_ADD_WDS_ENTRY_CMDID,
+        WMI_10X_PEER_REMOVE_WDS_ENTRY_CMDID,
+        WMI_10X_PEER_MCAST_GROUP_CMDID,
+
+        /* beacon/management specific commands */
+
+        WMI_10X_BCN_TX_CMDID,
+        WMI_10X_BCN_PRB_TMPL_CMDID,
+        WMI_10X_BCN_FILTER_RX_CMDID,
+        WMI_10X_PRB_REQ_FILTER_RX_CMDID,
+        WMI_10X_MGMT_TX_CMDID,
+
+        /* commands to directly control ba negotiation directly from host. */
+        WMI_10X_ADDBA_CLEAR_RESP_CMDID,
+        WMI_10X_ADDBA_SEND_CMDID,
+        WMI_10X_ADDBA_STATUS_CMDID,
+        WMI_10X_DELBA_SEND_CMDID,
+        WMI_10X_ADDBA_SET_RESP_CMDID,
+        WMI_10X_SEND_SINGLEAMSDU_CMDID,
+
+        /* Station power save specific config */
+        WMI_10X_STA_POWERSAVE_MODE_CMDID,
+        WMI_10X_STA_POWERSAVE_PARAM_CMDID,
+        WMI_10X_STA_MIMO_PS_MODE_CMDID,
+
+        /* set debug log config */
+        WMI_10X_DBGLOG_CFG_CMDID,
+
+        /* DFS-specific commands */
+        WMI_10X_PDEV_DFS_ENABLE_CMDID,
+        WMI_10X_PDEV_DFS_DISABLE_CMDID,
+
+        /* QVIT specific command id */
+        WMI_10X_PDEV_QVIT_CMDID,
+
+        /* Offload Scan and Roaming related  commands */
+        WMI_10X_ROAM_SCAN_MODE,
+        WMI_10X_ROAM_SCAN_RSSI_THRESHOLD,
+        WMI_10X_ROAM_SCAN_PERIOD,
+        WMI_10X_ROAM_SCAN_RSSI_CHANGE_THRESHOLD,
+        WMI_10X_ROAM_AP_PROFILE,
+        WMI_10X_OFL_SCAN_ADD_AP_PROFILE,
+        WMI_10X_OFL_SCAN_REMOVE_AP_PROFILE,
+        WMI_10X_OFL_SCAN_PERIOD,
+
+        /* P2P specific commands */
+        WMI_10X_P2P_DEV_SET_DEVICE_INFO,
+        WMI_10X_P2P_DEV_SET_DISCOVERABILITY,
+        WMI_10X_P2P_GO_SET_BEACON_IE,
+        WMI_10X_P2P_GO_SET_PROBE_RESP_IE,
+
+        /* AP power save specific config */
+        WMI_10X_AP_PS_PEER_PARAM_CMDID,
+        WMI_10X_AP_PS_PEER_UAPSD_COEX_CMDID,
+
+        /* Rate-control specific commands */
+        WMI_10X_PEER_RATE_RETRY_SCHED_CMDID,
+
+        /* WLAN Profiling commands. */
+        WMI_10X_WLAN_PROFILE_TRIGGER_CMDID,
+        WMI_10X_WLAN_PROFILE_SET_HIST_INTVL_CMDID,
+        WMI_10X_WLAN_PROFILE_GET_PROFILE_DATA_CMDID,
+        WMI_10X_WLAN_PROFILE_ENABLE_PROFILE_ID_CMDID,
+        WMI_10X_WLAN_PROFILE_LIST_PROFILE_ID_CMDID,
+
+        /* Suspend resume command Ids */
+        WMI_10X_PDEV_SUSPEND_CMDID,
+        WMI_10X_PDEV_RESUME_CMDID,
+
+        /* Beacon filter commands */
+        WMI_10X_ADD_BCN_FILTER_CMDID,
+        WMI_10X_RMV_BCN_FILTER_CMDID,
+
+        /* WOW Specific WMI commands*/
+        WMI_10X_WOW_ADD_WAKE_PATTERN_CMDID,
+        WMI_10X_WOW_DEL_WAKE_PATTERN_CMDID,
+        WMI_10X_WOW_ENABLE_DISABLE_WAKE_EVENT_CMDID,
+        WMI_10X_WOW_ENABLE_CMDID,
+        WMI_10X_WOW_HOSTWAKEUP_FROM_SLEEP_CMDID,
+
+        /* RTT measurement related cmd */
+        WMI_10X_RTT_MEASREQ_CMDID,
+        WMI_10X_RTT_TSF_CMDID,
+
+        /* transmit beacon by value */
+        WMI_10X_PDEV_SEND_BCN_CMDID,
+
+        /* F/W stats */
+        WMI_10X_VDEV_SPECTRAL_SCAN_CONFIGURE_CMDID,
+        WMI_10X_VDEV_SPECTRAL_SCAN_ENABLE_CMDID,
+        WMI_10X_REQUEST_STATS_CMDID,
+
+        /* GPIO Configuration */
+        WMI_10X_GPIO_CONFIG_CMDID,
+        WMI_10X_GPIO_OUTPUT_CMDID,
+
+        WMI_10X_PDEV_UTF_CMDID = WMI_10X_END_CMDID - 1,
+};
+
+enum wmi_10x_event_id {
+        WMI_10X_SERVICE_READY_EVENTID = 0x8000,
+        WMI_10X_READY_EVENTID,
+        WMI_10X_START_EVENTID = 0x9000,
+        WMI_10X_END_EVENTID = 0x9FFF,
+
+        /* Scan specific events */
+        WMI_10X_SCAN_EVENTID = WMI_10X_START_EVENTID,
+        WMI_10X_ECHO_EVENTID,
+        WMI_10X_DEBUG_MESG_EVENTID,
+        WMI_10X_UPDATE_STATS_EVENTID,
+
+        /* Instantaneous RSSI event */
+        WMI_10X_INST_RSSI_STATS_EVENTID,
+
+        /* VDEV specific events */
+        WMI_10X_VDEV_START_RESP_EVENTID,
+        WMI_10X_VDEV_STANDBY_REQ_EVENTID,
+        WMI_10X_VDEV_RESUME_REQ_EVENTID,
+        WMI_10X_VDEV_STOPPED_EVENTID,
+
+        /* peer  specific events */
+        WMI_10X_PEER_STA_KICKOUT_EVENTID,
+
+        /* beacon/mgmt specific events */
+        WMI_10X_HOST_SWBA_EVENTID,
+        WMI_10X_TBTTOFFSET_UPDATE_EVENTID,
+        WMI_10X_MGMT_RX_EVENTID,
+
+        /* Channel stats event */
+        WMI_10X_CHAN_INFO_EVENTID,
+
+        /* PHY Error specific WMI event */
+        WMI_10X_PHYERR_EVENTID,
+
+        /* Roam event to trigger roaming on host */
+        WMI_10X_ROAM_EVENTID,
+
+        /* matching AP found from list of profiles */
+        WMI_10X_PROFILE_MATCH,
+
+        /* debug print message used for tracing FW code while debugging */
+        WMI_10X_DEBUG_PRINT_EVENTID,
+        /* VI spoecific event */
+        WMI_10X_PDEV_QVIT_EVENTID,
+        /* FW code profile data in response to profile request */
+        WMI_10X_WLAN_PROFILE_DATA_EVENTID,
+
+        /*RTT related event ID*/
+        WMI_10X_RTT_MEASUREMENT_REPORT_EVENTID,
+        WMI_10X_TSF_MEASUREMENT_REPORT_EVENTID,
+        WMI_10X_RTT_ERROR_REPORT_EVENTID,
+
+        WMI_10X_WOW_WAKEUP_HOST_EVENTID,
+        WMI_10X_DCS_INTERFERENCE_EVENTID,
+
+        /* TPC config for the current operating channel */
+        WMI_10X_PDEV_TPC_CONFIG_EVENTID,
+
+        WMI_10X_GPIO_INPUT_EVENTID,
+        WMI_10X_PDEV_UTF_EVENTID = WMI_10X_END_EVENTID-1,
+};
+
 enum wmi_phy_mode {
         MODE_11A        = 0,   /* 11a Mode */
         MODE_11G        = 1,   /* 11b/g Mode */
@@ -805,6 +1130,46 @@ struct wmi_service_ready_event {
         struct wlan_host_mem_req mem_reqs[1];
 } __packed;
 
+/* This is the definition from 10.X firmware branch */
+struct wmi_service_ready_event_10x {
+        __le32 sw_version;
+        __le32 abi_version;
+
+        /* WMI_PHY_CAPABILITY */
+        __le32 phy_capability;
+
+        /* Maximum number of frag table entries that SW will populate less 1 */
+        __le32 max_frag_entry;
+        __le32 wmi_service_bitmap[WMI_SERVICE_BM_SIZE];
+        __le32 num_rf_chains;
+
+        /*
+         * The following field is only valid for service type
+         * WMI_SERVICE_11AC
+         */
+        __le32 ht_cap_info; /* WMI HT Capability */
+        __le32 vht_cap_info; /* VHT capability info field of 802.11ac */
+        __le32 vht_supp_mcs; /* VHT Supported MCS Set field Rx/Tx same */
+        __le32 hw_min_tx_power;
+        __le32 hw_max_tx_power;
+
+        struct hal_reg_capabilities hal_reg_capabilities;
+
+        __le32 sys_cap_info;
+        __le32 min_pkt_size_enable; /* Enterprise mode short pkt enable */
+
+        /*
+         * request to host to allocate a chuck of memory and pss it down to FW
+         * via WM_INIT. FW uses this as FW extesnsion memory for saving its
+         * data structures. Only valid for low latency interfaces like PCIE
+         * where FW can access this memory directly (or) by DMA.
+         */
+        __le32 num_mem_reqs;
+
+        struct wlan_host_mem_req mem_reqs[1];
+} __packed;
+
+
 #define WMI_SERVICE_READY_TIMEOUT_HZ (5*HZ)
 #define WMI_UNIFIED_READY_TIMEOUT_HZ (5*HZ)
 
@@ -1012,6 +1377,192 @@ struct wmi_resource_config {
         __le32 max_frag_entries;
 } __packed;
 
+struct wmi_resource_config_10x {
+        /* number of virtual devices (VAPs) to support */
+        __le32 num_vdevs;
+
+        /* number of peer nodes to support */
+        __le32 num_peers;
+
+        /* number of keys per peer */
+        __le32 num_peer_keys;
+
+        /* total number of TX/RX data TIDs */
+        __le32 num_tids;
+
+        /*
+         * max skid for resolving hash collisions
+         *
+         *   The address search table is sparse, so that if two MAC addresses
+         *   result in the same hash value, the second of these conflicting
+         *   entries can slide to the next index in the address search table,
+         *   and use it, if it is unoccupied.  This ast_skid_limit parameter
+         *   specifies the upper bound on how many subsequent indices to search
+         *   over to find an unoccupied space.
+         */
+        __le32 ast_skid_limit;
+
+        /*
+         * the nominal chain mask for transmit
+         *
+         *   The chain mask may be modified dynamically, e.g. to operate AP
+         *   tx with a reduced number of chains if no clients are associated.
+         *   This configuration parameter specifies the nominal chain-mask that
+         *   should be used when not operating with a reduced set of tx chains.
+         */
+        __le32 tx_chain_mask;
+
+        /*
+         * the nominal chain mask for receive
+         *
+         *   The chain mask may be modified dynamically, e.g. for a client
+         *   to use a reduced number of chains for receive if the traffic to
+         *   the client is low enough that it doesn't require downlink MIMO
+         *   or antenna diversity.
+         *   This configuration parameter specifies the nominal chain-mask that
+         *   should be used when not operating with a reduced set of rx chains.
+         */
+        __le32 rx_chain_mask;
+
+        /*
+         * what rx reorder timeout (ms) to use for the AC
+         *
+         *   Each WMM access class (voice, video, best-effort, background) will
+         *   have its own timeout value to dictate how long to wait for missing
+         *   rx MPDUs to arrive before flushing subsequent MPDUs that have
+         *   already been received.
+         *   This parameter specifies the timeout in milliseconds for each
+         *   class.
+         */
+        __le32 rx_timeout_pri_vi;
+        __le32 rx_timeout_pri_vo;
+        __le32 rx_timeout_pri_be;
+        __le32 rx_timeout_pri_bk;
+
+        /*
+         * what mode the rx should decap packets to
+         *
+         *   MAC can decap to RAW (no decap), native wifi or Ethernet types
+         *   THis setting also determines the default TX behavior, however TX
+         *   behavior can be modified on a per VAP basis during VAP init
+         */
+        __le32 rx_decap_mode;
+
+        /* what is the maximum scan requests than can be queued */
+        __le32 scan_max_pending_reqs;
+
+        /* maximum VDEV that could use BMISS offload */
+        __le32 bmiss_offload_max_vdev;
+
+        /* maximum VDEV that could use offload roaming */
+        __le32 roam_offload_max_vdev;
+
+        /* maximum AP profiles that would push to offload roaming */
+        __le32 roam_offload_max_ap_profiles;
+
+        /*
+         * how many groups to use for mcast->ucast conversion
+         *
+         *   The target's WAL maintains a table to hold information regarding
+         *   which peers belong to a given multicast group, so that if
+         *   multicast->unicast conversion is enabled, the target can convert
+         *   multicast tx frames to a series of unicast tx frames, to each
+         *   peer within the multicast group.
+             This num_mcast_groups configuration parameter tells the target how
+         *   many multicast groups to provide storage for within its multicast
+         *   group membership table.
+         */
+        __le32 num_mcast_groups;
+
+        /*
+         * size to alloc for the mcast membership table
+         *
+         *   This num_mcast_table_elems configuration parameter tells the
+         *   target how many peer elements it needs to provide storage for in
+         *   its multicast group membership table.
+         *   These multicast group membership table elements are shared by the
+         *   multicast groups stored within the table.
+         */
+        __le32 num_mcast_table_elems;
+
+        /*
+         * whether/how to do multicast->unicast conversion
+         *
+         *   This configuration parameter specifies whether the target should
+         *   perform multicast --> unicast conversion on transmit, and if so,
+         *   what to do if it finds no entries in its multicast group
+         *   membership table for the multicast IP address in the tx frame.
+         *   Configuration value:
+         *   0 -> Do not perform multicast to unicast conversion.
+         *   1 -> Convert multicast frames to unicast, if the IP multicast
+         *        address from the tx frame is found in the multicast group
+         *        membership table.  If the IP multicast address is not found,
+         *        drop the frame.
+         *   2 -> Convert multicast frames to unicast, if the IP multicast
+         *        address from the tx frame is found in the multicast group
+         *        membership table.  If the IP multicast address is not found,
+         *        transmit the frame as multicast.
+         */
+        __le32 mcast2ucast_mode;
+
+        /*
+         * how much memory to allocate for a tx PPDU dbg log
+         *
+         *   This parameter controls how much memory the target will allocate
+         *   to store a log of tx PPDU meta-information (how large the PPDU
+         *   was, when it was sent, whether it was successful, etc.)
+         */
+        __le32 tx_dbg_log_size;
+
+        /* how many AST entries to be allocated for WDS */
+        __le32 num_wds_entries;
+
+        /*
+         * MAC DMA burst size, e.g., For target PCI limit can be
+         * 0 -default, 1 256B
+         */
+        __le32 dma_burst_size;
+
+        /*
+         * Fixed delimiters to be inserted after every MPDU to
+         * account for interface latency to avoid underrun.
+         */
+        __le32 mac_aggr_delim;
+
+        /*
+         *   determine whether target is responsible for detecting duplicate
+         *   non-aggregate MPDU and timing out stale fragments.
+         *
+         *   A-MPDU reordering is always performed on the target.
+         *
+         *   0: target responsible for frag timeout and dup checking
+         *   1: host responsible for frag timeout and dup checking
+         */
+        __le32 rx_skip_defrag_timeout_dup_detection_check;
+
+        /*
+         * Configuration for VoW :
+         * No of Video Nodes to be supported
+         * and Max no of descriptors for each Video link (node).
+         */
+        __le32 vow_config;
+
+        /* Number of msdu descriptors target should use */
+        __le32 num_msdu_desc;
+
+        /*
+         * Max. number of Tx fragments per MSDU
+         *  This parameter controls the max number of Tx fragments per MSDU.
+         *  This is sent by the target as part of the WMI_SERVICE_READY event
+         *  and is overriden by the OS shim as required.
+         */
+        __le32 max_frag_entries;
+} __packed;
+
+
+#define NUM_UNITS_IS_NUM_VDEVS   0x1
+#define NUM_UNITS_IS_NUM_PEERS   0x2
+
 /* strucutre describing host memory chunk. */
 struct host_memory_chunk {
         /* id of the request that is passed up in service ready */
@@ -1033,6 +1584,18 @@ struct wmi_init_cmd {
         struct host_memory_chunk host_mem_chunks[1];
 } __packed;
 
+/* _10x stucture is from 10.X FW API */
+struct wmi_init_cmd_10x {
+        struct wmi_resource_config_10x resource_config;
+        __le32 num_host_mem_chunks;
+
+        /*
+         * variable number of host memory chunks.
+         * This should be the last element in the structure
+         */
+        struct host_memory_chunk host_mem_chunks[1];
+} __packed;
+
 /* TLV for channel list */
 struct wmi_chan_list {
         __le32 tag; /* WMI_CHAN_LIST_TAG */
@@ -1152,6 +1715,88 @@ struct wmi_start_scan_cmd {
          */
 } __packed;
 
+/* This is the definition from 10.X firmware branch */
+struct wmi_start_scan_cmd_10x {
+        /* Scan ID */
+        __le32 scan_id;
+
+        /* Scan requestor ID */
+        __le32 scan_req_id;
+
+        /* VDEV id(interface) that is requesting scan */
+        __le32 vdev_id;
+
+        /* Scan Priority, input to scan scheduler */
+        __le32 scan_priority;
+
+        /* Scan events subscription */
+        __le32 notify_scan_events;
+
+        /* dwell time in msec on active channels */
+        __le32 dwell_time_active;
+
+        /* dwell time in msec on passive channels */
+        __le32 dwell_time_passive;
+
+        /*
+         * min time in msec on the BSS channel,only valid if atleast one
+         * VDEV is active
+         */
+        __le32 min_rest_time;
+
+        /*
+         * max rest time in msec on the BSS channel,only valid if at least
+         * one VDEV is active
+         */
+        /*
+         * the scanner will rest on the bss channel at least min_rest_time
+         * after min_rest_time the scanner will start checking for tx/rx
+         * activity on all VDEVs. if there is no activity the scanner will
+         * switch to off channel. if there is activity the scanner will let
+         * the radio on the bss channel until max_rest_time expires.at
+         * max_rest_time scanner will switch to off channel irrespective of
+         * activity. activity is determined by the idle_time parameter.
+         */
+        __le32 max_rest_time;
+
+        /*
+         * time before sending next set of probe requests.
+         * The scanner keeps repeating probe requests transmission with
+         * period specified by repeat_probe_time.
+         * The number of probe requests specified depends on the ssid_list
+         * and bssid_list
+         */
+        __le32 repeat_probe_time;
+
+        /* time in msec between 2 consequetive probe requests with in a set. */
+        __le32 probe_spacing_time;
+
+        /*
+         * data inactivity time in msec on bss channel that will be used by
+         * scanner for measuring the inactivity.
+         */
+        __le32 idle_time;
+
+        /* maximum time in msec allowed for scan  */
+        __le32 max_scan_time;
+
+        /*
+         * delay in msec before sending first probe request after switching
+         * to a channel
+         */
+        __le32 probe_delay;
+
+        /* Scan control flags */
+        __le32 scan_ctrl_flags;
+
+        /*
+         * TLV (tag length value )  paramerters follow the scan_cmd structure.
+         * TLV can contain channel list, bssid list, ssid list and
+         * ie. the TLV tags are defined above;
+         */
+} __packed;
+
+
 struct wmi_ssid_arg {
         int len;
         const u8 *ssid;
@@ -1509,6 +2154,60 @@ struct wmi_csa_event {
 #define VDEV_DEFAULT_STATS_UPDATE_PERIOD    500
 #define PEER_DEFAULT_STATS_UPDATE_PERIOD    500
 
+struct wmi_pdev_param_map {
+        u32 tx_chain_mask;
+        u32 rx_chain_mask;
+        u32 txpower_limit2g;
+        u32 txpower_limit5g;
+        u32 txpower_scale;
+        u32 beacon_gen_mode;
+        u32 beacon_tx_mode;
+        u32 resmgr_offchan_mode;
+        u32 protection_mode;
+        u32 dynamic_bw;
+        u32 non_agg_sw_retry_th;
+        u32 agg_sw_retry_th;
+        u32 sta_kickout_th;
+        u32 ac_aggrsize_scaling;
+        u32 ltr_enable;
+        u32 ltr_ac_latency_be;
+        u32 ltr_ac_latency_bk;
+        u32 ltr_ac_latency_vi;
+        u32 ltr_ac_latency_vo;
+        u32 ltr_ac_latency_timeout;
+        u32 ltr_sleep_override;
+        u32 ltr_rx_override;
+        u32 ltr_tx_activity_timeout;
+        u32 l1ss_enable;
+        u32 dsleep_enable;
+        u32 pcielp_txbuf_flush;
+        u32 pcielp_txbuf_watermark;
+        u32 pcielp_txbuf_tmo_en;
+        u32 pcielp_txbuf_tmo_value;
+        u32 pdev_stats_update_period;
+        u32 vdev_stats_update_period;
+        u32 peer_stats_update_period;
+        u32 bcnflt_stats_update_period;
+        u32 pmf_qos;
+        u32 arp_ac_override;
+        u32 arpdhcp_ac_override;
+        u32 dcs;
+        u32 ani_enable;
+        u32 ani_poll_period;
+        u32 ani_listen_period;
+        u32 ani_ofdm_level;
+        u32 ani_cck_level;
+        u32 dyntxchain;
+        u32 proxy_sta;
+        u32 idle_ps_config;
+        u32 power_gating_sleep;
+        u32 fast_channel_reset;
+        u32 burst_dur;
+        u32 burst_enable;
+};
+
+#define WMI_PDEV_PARAM_UNSUPPORTED 0
+
 enum wmi_pdev_param {
         /* TX chian mask */
         WMI_PDEV_PARAM_TX_CHAIN_MASK = 0x1,
@@ -1608,6 +2307,97 @@ enum wmi_pdev_param {
         WMI_PDEV_PARAM_POWER_GATING_SLEEP,
 };
 
+enum wmi_10x_pdev_param {
+        /* TX chian mask */
+        WMI_10X_PDEV_PARAM_TX_CHAIN_MASK = 0x1,
+        /* RX chian mask */
+        WMI_10X_PDEV_PARAM_RX_CHAIN_MASK,
+        /* TX power limit for 2G Radio */
+        WMI_10X_PDEV_PARAM_TXPOWER_LIMIT2G,
+        /* TX power limit for 5G Radio */
+        WMI_10X_PDEV_PARAM_TXPOWER_LIMIT5G,
+        /* TX power scale */
+        WMI_10X_PDEV_PARAM_TXPOWER_SCALE,
+        /* Beacon generation mode . 0: host, 1: target   */
+        WMI_10X_PDEV_PARAM_BEACON_GEN_MODE,
+        /* Beacon generation mode . 0: staggered 1: bursted   */
+        WMI_10X_PDEV_PARAM_BEACON_TX_MODE,
+        /*
+         * Resource manager off chan mode .
+         * 0: turn off off chan mode. 1: turn on offchan mode
+         */
+        WMI_10X_PDEV_PARAM_RESMGR_OFFCHAN_MODE,
+        /*
+         * Protection mode:
+         * 0: no protection 1:use CTS-to-self 2: use RTS/CTS
+         */
+        WMI_10X_PDEV_PARAM_PROTECTION_MODE,
+        /* Dynamic bandwidth 0: disable 1: enable */
+        WMI_10X_PDEV_PARAM_DYNAMIC_BW,
+        /* Non aggregrate/ 11g sw retry threshold.0-disable */
+        WMI_10X_PDEV_PARAM_NON_AGG_SW_RETRY_TH,
+        /* aggregrate sw retry threshold. 0-disable*/
+        WMI_10X_PDEV_PARAM_AGG_SW_RETRY_TH,
+        /* Station kickout threshold (non of consecutive failures).0-disable */
+        WMI_10X_PDEV_PARAM_STA_KICKOUT_TH,
+        /* Aggerate size scaling configuration per AC */
+        WMI_10X_PDEV_PARAM_AC_AGGRSIZE_SCALING,
+        /* LTR enable */
+        WMI_10X_PDEV_PARAM_LTR_ENABLE,
+        /* LTR latency for BE, in us */
+        WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_BE,
+        /* LTR latency for BK, in us */
+        WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_BK,
+        /* LTR latency for VI, in us */
+        WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_VI,
+        /* LTR latency for VO, in us  */
+        WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_VO,
+        /* LTR AC latency timeout, in ms */
+        WMI_10X_PDEV_PARAM_LTR_AC_LATENCY_TIMEOUT,
+        /* LTR platform latency override, in us */
+        WMI_10X_PDEV_PARAM_LTR_SLEEP_OVERRIDE,
+        /* LTR-RX override, in us */
+        WMI_10X_PDEV_PARAM_LTR_RX_OVERRIDE,
+        /* Tx activity timeout for LTR, in us */
+        WMI_10X_PDEV_PARAM_LTR_TX_ACTIVITY_TIMEOUT,
+        /* L1SS state machine enable */
+        WMI_10X_PDEV_PARAM_L1SS_ENABLE,
+        /* Deep sleep state machine enable */
+        WMI_10X_PDEV_PARAM_DSLEEP_ENABLE,
+        /* pdev level stats update period in ms */
+        WMI_10X_PDEV_PARAM_PDEV_STATS_UPDATE_PERIOD,
+        /* vdev level stats update period in ms */
+        WMI_10X_PDEV_PARAM_VDEV_STATS_UPDATE_PERIOD,
+        /* peer level stats update period in ms */
+        WMI_10X_PDEV_PARAM_PEER_STATS_UPDATE_PERIOD,
+        /* beacon filter status update period */
+        WMI_10X_PDEV_PARAM_BCNFLT_STATS_UPDATE_PERIOD,
+        /* QOS Mgmt frame protection MFP/PMF 0: disable, 1: enable */
+        WMI_10X_PDEV_PARAM_PMF_QOS,
+        /* Access category on which ARP and DHCP frames are sent */
+        WMI_10X_PDEV_PARAM_ARPDHCP_AC_OVERRIDE,
+        /* DCS configuration */
+        WMI_10X_PDEV_PARAM_DCS,
+        /* Enable/Disable ANI on target */
+        WMI_10X_PDEV_PARAM_ANI_ENABLE,
+        /* configure the ANI polling period */
+        WMI_10X_PDEV_PARAM_ANI_POLL_PERIOD,
+        /* configure the ANI listening period */
+        WMI_10X_PDEV_PARAM_ANI_LISTEN_PERIOD,
+        /* configure OFDM immunity level */
+        WMI_10X_PDEV_PARAM_ANI_OFDM_LEVEL,
+        /* configure CCK immunity level */
+        WMI_10X_PDEV_PARAM_ANI_CCK_LEVEL,
+        /* Enable/Disable CDD for 1x1 STAs in rate control module */
+        WMI_10X_PDEV_PARAM_DYNTXCHAIN,
+        /* Enable/Disable Fast channel reset*/
+        WMI_10X_PDEV_PARAM_FAST_CHANNEL_RESET,
+        /* Set Bursting DUR */
+        WMI_10X_PDEV_PARAM_BURST_DUR,
+        /* Set Bursting Enable*/
+        WMI_10X_PDEV_PARAM_BURST_ENABLE,
+};
+
 struct wmi_pdev_set_param_cmd {
         __le32 param_id;
         __le32 param_value;
@@ -2132,6 +2922,61 @@ enum wmi_rate_preamble {
 /* Value to disable fixed rate setting */
 #define WMI_FIXED_RATE_NONE    (0xff)
 
+struct wmi_vdev_param_map {
+        u32 rts_threshold;
+        u32 fragmentation_threshold;
+        u32 beacon_interval;
+        u32 listen_interval;
+        u32 multicast_rate;
+        u32 mgmt_tx_rate;
+        u32 slot_time;
+        u32 preamble;
+        u32 swba_time;
+        u32 wmi_vdev_stats_update_period;
+        u32 wmi_vdev_pwrsave_ageout_time;
+        u32 wmi_vdev_host_swba_interval;
+        u32 dtim_period;
+        u32 wmi_vdev_oc_scheduler_air_time_limit;
+        u32 wds;
+        u32 atim_window;
+        u32 bmiss_count_max;
+        u32 bmiss_first_bcnt;
+        u32 bmiss_final_bcnt;
+        u32 feature_wmm;
+        u32 chwidth;
+        u32 chextoffset;
+        u32 disable_htprotection;
+        u32 sta_quickkickout;
+        u32 mgmt_rate;
+        u32 protection_mode;
+        u32 fixed_rate;
+        u32 sgi;
+        u32 ldpc;
+        u32 tx_stbc;
+        u32 rx_stbc;
+        u32 intra_bss_fwd;
+        u32 def_keyid;
+        u32 nss;
+        u32 bcast_data_rate;
+        u32 mcast_data_rate;
+        u32 mcast_indicate;
+        u32 dhcp_indicate;
+        u32 unknown_dest_indicate;
+        u32 ap_keepalive_min_idle_inactive_time_secs;
+        u32 ap_keepalive_max_idle_inactive_time_secs;
+        u32 ap_keepalive_max_unresponsive_time_secs;
+        u32 ap_enable_nawds;
+        u32 mcast2ucast_set;
+        u32 enable_rtscts;
+        u32 txbf;
+        u32 packet_powersave;
+        u32 drop_unencry;
+        u32 tx_encap_type;
+        u32 ap_detect_out_of_sync_sleeping_sta_time_secs;
+};
+
+#define WMI_VDEV_PARAM_UNSUPPORTED 0
+
 /* the definition of different VDEV parameters */
 enum wmi_vdev_param {
         /* RTS Threshold */
@@ -2263,6 +3108,121 @@ enum wmi_vdev_param {
         WMI_VDEV_PARAM_TX_ENCAP_TYPE,
 };
 
+/* the definition of different VDEV parameters */
+enum wmi_10x_vdev_param {
+        /* RTS Threshold */
+        WMI_10X_VDEV_PARAM_RTS_THRESHOLD = 0x1,
+        /* Fragmentation threshold */
+        WMI_10X_VDEV_PARAM_FRAGMENTATION_THRESHOLD,
+        /* beacon interval in TUs */
+        WMI_10X_VDEV_PARAM_BEACON_INTERVAL,
+        /* Listen interval in TUs */
+        WMI_10X_VDEV_PARAM_LISTEN_INTERVAL,
+        /* muticast rate in Mbps */
+        WMI_10X_VDEV_PARAM_MULTICAST_RATE,
+        /* management frame rate in Mbps */
+        WMI_10X_VDEV_PARAM_MGMT_TX_RATE,
+        /* slot time (long vs short) */
+        WMI_10X_VDEV_PARAM_SLOT_TIME,
+        /* preamble (long vs short) */
+        WMI_10X_VDEV_PARAM_PREAMBLE,
+        /* SWBA time (time before tbtt in msec) */
+        WMI_10X_VDEV_PARAM_SWBA_TIME,
+        /* time period for updating VDEV stats */
+        WMI_10X_VDEV_STATS_UPDATE_PERIOD,
+        /* age out time in msec for frames queued for station in power save */
+        WMI_10X_VDEV_PWRSAVE_AGEOUT_TIME,
+        /*
+         * Host SWBA interval (time in msec before tbtt for SWBA event
+         * generation).
+         */
+        WMI_10X_VDEV_HOST_SWBA_INTERVAL,
+        /* DTIM period (specified in units of num beacon intervals) */
+        WMI_10X_VDEV_PARAM_DTIM_PERIOD,
+        /*
+         * scheduler air time limit for this VDEV. used by off chan
+         * scheduler.
+         */
+        WMI_10X_VDEV_OC_SCHEDULER_AIR_TIME_LIMIT,
+        /* enable/dsiable WDS for this VDEV  */
+        WMI_10X_VDEV_PARAM_WDS,
+        /* ATIM Window */
+        WMI_10X_VDEV_PARAM_ATIM_WINDOW,
+        /* BMISS max */
+        WMI_10X_VDEV_PARAM_BMISS_COUNT_MAX,
+        /* WMM enables/disabled */
+        WMI_10X_VDEV_PARAM_FEATURE_WMM,
+        /* Channel width */
+        WMI_10X_VDEV_PARAM_CHWIDTH,
+        /* Channel Offset */
+        WMI_10X_VDEV_PARAM_CHEXTOFFSET,
+        /* Disable HT Protection */
+        WMI_10X_VDEV_PARAM_DISABLE_HTPROTECTION,
+        /* Quick STA Kickout */
+        WMI_10X_VDEV_PARAM_STA_QUICKKICKOUT,
+        /* Rate to be used with Management frames */
+        WMI_10X_VDEV_PARAM_MGMT_RATE,
+        /* Protection Mode */
+        WMI_10X_VDEV_PARAM_PROTECTION_MODE,
+        /* Fixed rate setting */
+        WMI_10X_VDEV_PARAM_FIXED_RATE,
+        /* Short GI Enable/Disable */
+        WMI_10X_VDEV_PARAM_SGI,
+        /* Enable LDPC */
+        WMI_10X_VDEV_PARAM_LDPC,
+        /* Enable Tx STBC */
+        WMI_10X_VDEV_PARAM_TX_STBC,
+        /* Enable Rx STBC */
+        WMI_10X_VDEV_PARAM_RX_STBC,
+        /* Intra BSS forwarding  */
+        WMI_10X_VDEV_PARAM_INTRA_BSS_FWD,
+        /* Setting Default xmit key for Vdev */
+        WMI_10X_VDEV_PARAM_DEF_KEYID,
+        /* NSS width */
+        WMI_10X_VDEV_PARAM_NSS,
+        /* Set the custom rate for the broadcast data frames */
+        WMI_10X_VDEV_PARAM_BCAST_DATA_RATE,
+        /* Set the custom rate (rate-code) for multicast data frames */
+        WMI_10X_VDEV_PARAM_MCAST_DATA_RATE,
+        /* Tx multicast packet indicate Enable/Disable */
+        WMI_10X_VDEV_PARAM_MCAST_INDICATE,
+        /* Tx DHCP packet indicate Enable/Disable */
+        WMI_10X_VDEV_PARAM_DHCP_INDICATE,
+        /* Enable host inspection of Tx unicast packet to unknown destination */
+        WMI_10X_VDEV_PARAM_UNKNOWN_DEST_INDICATE,
+
+        /* The minimum amount of time AP begins to consider STA inactive */
+        WMI_10X_VDEV_PARAM_AP_KEEPALIVE_MIN_IDLE_INACTIVE_TIME_SECS,
+
+        /*
+         * An associated STA is considered inactive when there is no recent
+         * TX/RX activity and no downlink frames are buffered for it. Once a
+         * STA exceeds the maximum idle inactive time, the AP will send an
+         * 802.11 data-null as a keep alive to verify the STA is still
+         * associated. If the STA does ACK the data-null, or if the data-null
+         * is buffered and the STA does not retrieve it, the STA will be
+         * considered unresponsive
+         * (see WMI_10X_VDEV_AP_KEEPALIVE_MAX_UNRESPONSIVE_TIME_SECS).
+         */
+        WMI_10X_VDEV_PARAM_AP_KEEPALIVE_MAX_IDLE_INACTIVE_TIME_SECS,
+
+        /*
+         * An associated STA is considered unresponsive if there is no recent
+         * TX/RX activity and downlink frames are buffered for it. Once a STA
+         * exceeds the maximum unresponsive time, the AP will send a
+         * WMI_10X_STA_KICKOUT event to the host so the STA can be deleted. */
+        WMI_10X_VDEV_PARAM_AP_KEEPALIVE_MAX_UNRESPONSIVE_TIME_SECS,
+
+        /* Enable NAWDS : MCAST INSPECT Enable, NAWDS Flag set */
+        WMI_10X_VDEV_PARAM_AP_ENABLE_NAWDS,
+
+        WMI_10X_VDEV_PARAM_MCAST2UCAST_SET,
+        /* Enable/Disable RTS-CTS */
+        WMI_10X_VDEV_PARAM_ENABLE_RTSCTS,
+
+        WMI_10X_VDEV_PARAM_AP_DETECT_OUT_OF_SYNC_SLEEPING_STA_TIME_SECS,
+};
+
 /* slot time long */
 #define WMI_VDEV_SLOT_TIME_LONG         0x1
 /* slot time short */
@@ -3064,8 +4024,7 @@ int ath10k_wmi_pdev_suspend_target(struct ath10k *ar);
 int ath10k_wmi_pdev_resume_target(struct ath10k *ar);
 int ath10k_wmi_pdev_set_regdomain(struct ath10k *ar, u16 rd, u16 rd2g,
                                   u16 rd5g, u16 ctl2g, u16 ctl5g);
-int ath10k_wmi_pdev_set_param(struct ath10k *ar, enum wmi_pdev_param id,
-                              u32 value);
+int ath10k_wmi_pdev_set_param(struct ath10k *ar, u32 id, u32 value);
 int ath10k_wmi_cmd_init(struct ath10k *ar);
 int ath10k_wmi_start_scan(struct ath10k *ar, const struct wmi_start_scan_arg *);
 void ath10k_wmi_start_scan_init(struct ath10k *ar, struct wmi_start_scan_arg *);
@@ -3085,7 +4044,7 @@ int ath10k_wmi_vdev_up(struct ath10k *ar, u32 vdev_id, u32 aid,
                        const u8 *bssid);
 int ath10k_wmi_vdev_down(struct ath10k *ar, u32 vdev_id);
 int ath10k_wmi_vdev_set_param(struct ath10k *ar, u32 vdev_id,
-                              enum wmi_vdev_param param_id, u32 param_value);
+                              u32 param_id, u32 param_value);
 int ath10k_wmi_vdev_install_key(struct ath10k *ar,
                                 const struct wmi_vdev_install_key_arg *arg);
 int ath10k_wmi_peer_create(struct ath10k *ar, u32 vdev_id,
@@ -3115,5 +4074,6 @@ int ath10k_wmi_pdev_set_wmm_params(struct ath10k *ar,
 int ath10k_wmi_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id);
 int ath10k_wmi_force_fw_hang(struct ath10k *ar,
                              enum wmi_force_fw_hang_type type, u32 delay_ms);
+int ath10k_wmi_mgmt_tx(struct ath10k *ar, struct sk_buff *skb);
 
 #endif /* _WMI_H_ */
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index 48161edec8de..69f58b073e85 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -1663,15 +1663,15 @@ ath5k_tx_frame_completed(struct ath5k_hw *ah, struct sk_buff *skb,
         ah->stats.tx_bytes_count += skb->len;
         info = IEEE80211_SKB_CB(skb);
 
+        size = min_t(int, sizeof(info->status.rates), sizeof(bf->rates));
+        memcpy(info->status.rates, bf->rates, size);
+
         tries[0] = info->status.rates[0].count;
         tries[1] = info->status.rates[1].count;
         tries[2] = info->status.rates[2].count;
 
         ieee80211_tx_info_clear_status(info);
 
-        size = min_t(int, sizeof(info->status.rates), sizeof(bf->rates));
-        memcpy(info->status.rates, bf->rates, size);
-
         for (i = 0; i < ts->ts_final_idx; i++) {
                 struct ieee80211_tx_rate *r =
                         &info->status.rates[i];
diff --git a/drivers/net/wireless/ath/ath6kl/htc.h b/drivers/net/wireless/ath/ath6kl/htc.h
index a2c8ff809793..14cab1403dd6 100644
--- a/drivers/net/wireless/ath/ath6kl/htc.h
+++ b/drivers/net/wireless/ath/ath6kl/htc.h
@@ -60,7 +60,7 @@
 /* disable credit flow control on a specific service */
 #define HTC_CONN_FLGS_DISABLE_CRED_FLOW_CTRL          (1 << 3)
 #define HTC_CONN_FLGS_SET_RECV_ALLOC_SHIFT    8
-#define HTC_CONN_FLGS_SET_RECV_ALLOC_MASK     0xFF00
+#define HTC_CONN_FLGS_SET_RECV_ALLOC_MASK     0xFF00U
 
 /* connect response status codes */
 #define HTC_SERVICE_SUCCESS      0
diff --git a/drivers/net/wireless/ath/ath9k/Kconfig b/drivers/net/wireless/ath/ath9k/Kconfig
index 7944c25c9a43..32f139e2e897 100644
--- a/drivers/net/wireless/ath/ath9k/Kconfig
+++ b/drivers/net/wireless/ath/ath9k/Kconfig
@@ -84,6 +84,26 @@ config ATH9K_DFS_CERTIFIED
           developed. At this point enabling this option won't do anything
           except increase code size.
 
+config ATH9K_TX99
+        bool "Atheros ath9k TX99 testing support"
+        depends on CFG80211_CERTIFICATION_ONUS
+        default n
+        ---help---
+          Say N. This should only be enabled on systems undergoing
+          certification testing and evaluation in a controlled environment.
+          Enabling this will only enable TX99 support, all other modes of
+          operation will be disabled.
+
+          TX99 support enables Specific Absorption Rate (SAR) testing.
+          SAR is the unit of measurement for the amount of radio frequency(RF)
+          absorbed by the body when using a wireless device. The RF exposure
+          limits used are expressed in the terms of SAR, which is a measure
+          of the electric and magnetic field strength and power density for
+          transmitters operating at frequencies from 300 kHz to 100 GHz.
+          Regulatory bodies around the world require that wireless device
+          be evaluated to meet the RF exposure limits set forth in the
+          governmental SAR regulations.
+
 config ATH9K_LEGACY_RATE_CONTROL
         bool "Atheros ath9k rate control"
         depends on ATH9K
diff --git a/drivers/net/wireless/ath/ath9k/Makefile b/drivers/net/wireless/ath/ath9k/Makefile
index 75ee9e7704ce..6205ef5a9321 100644
--- a/drivers/net/wireless/ath/ath9k/Makefile
+++ b/drivers/net/wireless/ath/ath9k/Makefile
@@ -14,9 +14,7 @@ ath9k-$(CONFIG_ATH9K_AHB) += ahb.o
 ath9k-$(CONFIG_ATH9K_DEBUGFS) += debug.o
 ath9k-$(CONFIG_ATH9K_DFS_DEBUGFS) += dfs_debug.o
 ath9k-$(CONFIG_ATH9K_DFS_CERTIFIED) += \
-                dfs.o \
-                dfs_pattern_detector.o \
-                dfs_pri_detector.o
+                dfs.o
 ath9k-$(CONFIG_PM_SLEEP) += wow.o
 
 obj-$(CONFIG_ATH9K) += ath9k.o
diff --git a/drivers/net/wireless/ath/ath9k/ar9002_phy.c b/drivers/net/wireless/ath/ath9k/ar9002_phy.c
index 17970d49d858..f087117b2e6b 100644
--- a/drivers/net/wireless/ath/ath9k/ar9002_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar9002_phy.c
@@ -680,6 +680,26 @@ static void ar9002_hw_spectral_scan_wait(struct ath_hw *ah)
         }
 }
 
+static void ar9002_hw_tx99_start(struct ath_hw *ah, u32 qnum)
+{
+        REG_SET_BIT(ah, 0x9864, 0x7f000);
+        REG_SET_BIT(ah, 0x9924, 0x7f00fe);
+        REG_CLR_BIT(ah, AR_DIAG_SW, AR_DIAG_RX_DIS);
+        REG_WRITE(ah, AR_CR, AR_CR_RXD);
+        REG_WRITE(ah, AR_DLCL_IFS(qnum), 0);
+        REG_WRITE(ah, AR_D_GBL_IFS_SIFS, 20);
+        REG_WRITE(ah, AR_D_GBL_IFS_EIFS, 20);
+        REG_WRITE(ah, AR_D_FPCTL, 0x10|qnum);
+        REG_WRITE(ah, AR_TIME_OUT, 0x00000400);
+        REG_WRITE(ah, AR_DRETRY_LIMIT(qnum), 0xffffffff);
+        REG_SET_BIT(ah, AR_QMISC(qnum), AR_Q_MISC_DCU_EARLY_TERM_REQ);
+}
+
+static void ar9002_hw_tx99_stop(struct ath_hw *ah)
+{
+        REG_SET_BIT(ah, AR_DIAG_SW, AR_DIAG_RX_DIS);
+}
+
 void ar9002_hw_attach_phy_ops(struct ath_hw *ah)
 {
         struct ath_hw_private_ops *priv_ops = ath9k_hw_private_ops(ah);
@@ -701,6 +721,8 @@ void ar9002_hw_attach_phy_ops(struct ath_hw *ah)
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
         ops->set_bt_ant_diversity = ar9002_hw_set_bt_ant_diversity;
 #endif
+        ops->tx99_start = ar9002_hw_tx99_start;
+        ops->tx99_stop = ar9002_hw_tx99_stop;
 
         ar9002_hw_set_nf_limits(ah);
 }
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.c b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
index f3adafd33704..11f53589a3f3 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
@@ -1617,6 +1617,98 @@ static void ar9003_hw_spectral_scan_wait(struct ath_hw *ah)
         }
 }
 
+static void ar9003_hw_tx99_start(struct ath_hw *ah, u32 qnum)
+{
+        REG_SET_BIT(ah, AR_PHY_TEST, PHY_AGC_CLR);
+        REG_SET_BIT(ah, 0x9864, 0x7f000);
+        REG_SET_BIT(ah, 0x9924, 0x7f00fe);
+        REG_CLR_BIT(ah, AR_DIAG_SW, AR_DIAG_RX_DIS);
+        REG_WRITE(ah, AR_CR, AR_CR_RXD);
+        REG_WRITE(ah, AR_DLCL_IFS(qnum), 0);
+        REG_WRITE(ah, AR_D_GBL_IFS_SIFS, 20); /* 50 OK */
+        REG_WRITE(ah, AR_D_GBL_IFS_EIFS, 20);
+        REG_WRITE(ah, AR_TIME_OUT, 0x00000400);
+        REG_WRITE(ah, AR_DRETRY_LIMIT(qnum), 0xffffffff);
+        REG_SET_BIT(ah, AR_QMISC(qnum), AR_Q_MISC_DCU_EARLY_TERM_REQ);
+}
+
+static void ar9003_hw_tx99_stop(struct ath_hw *ah)
+{
+        REG_CLR_BIT(ah, AR_PHY_TEST, PHY_AGC_CLR);
+        REG_SET_BIT(ah, AR_DIAG_SW, AR_DIAG_RX_DIS);
+}
+
+static void ar9003_hw_tx99_set_txpower(struct ath_hw *ah, u8 txpower)
+{
+        static s16 p_pwr_array[ar9300RateSize] = { 0 };
+        unsigned int i;
+
+        if (txpower <= MAX_RATE_POWER) {
+                for (i = 0; i < ar9300RateSize; i++)
+                        p_pwr_array[i] = txpower;
+        } else {
+                for (i = 0; i < ar9300RateSize; i++)
+                        p_pwr_array[i] = MAX_RATE_POWER;
+        }
+
+        REG_WRITE(ah, 0xa458, 0);
+
+        REG_WRITE(ah, 0xa3c0,
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_6_24], 24) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_6_24], 16) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_6_24],  8) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_6_24],  0));
+        REG_WRITE(ah, 0xa3c4,
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_54],  24) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_48],  16) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_36],   8) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_6_24], 0));
+        REG_WRITE(ah, 0xa3c8,
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_1L_5L], 24) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_1L_5L], 16) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_1L_5L],  0));
+        REG_WRITE(ah, 0xa3cc,
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_11S],   24) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_11L],   16) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_5S],     8) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_LEGACY_1L_5L],  0));
+        REG_WRITE(ah, 0xa3d0,
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_5],  24) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_4],  16) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_1_3_9_11_17_19], 8)|
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_0_8_16], 0));
+        REG_WRITE(ah, 0xa3d4,
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_13], 24) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_12], 16) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_7],   8) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_6],   0));
+        REG_WRITE(ah, 0xa3e4,
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_21], 24) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_20], 16) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_15],  8) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_14],  0));
+        REG_WRITE(ah, 0xa3e8,
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_23], 24) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_22], 16) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_23],  8) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT20_22],  0));
+        REG_WRITE(ah, 0xa3d8,
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_5], 24) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_4], 16) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_1_3_9_11_17_19], 8) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_0_8_16], 0));
+        REG_WRITE(ah, 0xa3dc,
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_13], 24) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_12], 16) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_7],   8) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_6],   0));
+        REG_WRITE(ah, 0xa3ec,
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_21], 24) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_20], 16) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_15],  8) |
+                  ATH9K_POW_SM(p_pwr_array[ALL_TARGET_HT40_14],  0));
+}
+
 void ar9003_hw_attach_phy_ops(struct ath_hw *ah)
 {
         struct ath_hw_private_ops *priv_ops = ath9k_hw_private_ops(ah);
@@ -1656,6 +1748,9 @@ void ar9003_hw_attach_phy_ops(struct ath_hw *ah)
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
         ops->set_bt_ant_diversity = ar9003_hw_set_bt_ant_diversity;
 #endif
+        ops->tx99_start = ar9003_hw_tx99_start;
+        ops->tx99_stop = ar9003_hw_tx99_stop;
+        ops->tx99_set_txpower = ar9003_hw_tx99_set_txpower;
 
         ar9003_hw_set_nf_limits(ah);
         ar9003_hw_set_radar_conf(ah);
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index 14ff7e9dde4c..e7a38d844a6a 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -778,6 +778,11 @@ struct ath_softc {
         enum spectral_mode spectral_mode;
         struct ath_spec_scan spec_config;
 
+        struct ieee80211_vif *tx99_vif;
+        struct sk_buff *tx99_skb;
+        bool tx99_state;
+        s16 tx99_power;
+
 #ifdef CONFIG_PM_SLEEP
         atomic_t wow_got_bmiss_intr;
         atomic_t wow_sleep_proc_intr; /* in the middle of WoW sleep ? */
@@ -886,6 +891,7 @@ static inline u8 spectral_bitmap_weight(u8 *bins)
  */
 enum ath_fft_sample_type {
         ATH_FFT_SAMPLE_HT20 = 1,
+        ATH_FFT_SAMPLE_HT20_40,
 };
 
 struct fft_sample_tlv {
@@ -912,6 +918,39 @@ struct fft_sample_ht20 {
         u8 data[SPECTRAL_HT20_NUM_BINS];
 } __packed;
 
+struct fft_sample_ht20_40 {
+        struct fft_sample_tlv tlv;
+
+        u8 channel_type;
+        __be16 freq;
+
+        s8 lower_rssi;
+        s8 upper_rssi;
+
+        __be64 tsf;
+
+        s8 lower_noise;
+        s8 upper_noise;
+
+        __be16 lower_max_magnitude;
+        __be16 upper_max_magnitude;
+
+        u8 lower_max_index;
+        u8 upper_max_index;
+
+        u8 lower_bitmap_weight;
+        u8 upper_bitmap_weight;
+
+        u8 max_exp;
+
+        u8 data[SPECTRAL_HT20_40_NUM_BINS];
+} __packed;
+
+int ath9k_tx99_init(struct ath_softc *sc);
+void ath9k_tx99_deinit(struct ath_softc *sc);
+int ath9k_tx99_send(struct ath_softc *sc, struct sk_buff *skb,
+                    struct ath_tx_control *txctl);
+
 void ath9k_tasklet(unsigned long data);
 int ath_cabq_update(struct ath_softc *);
 
diff --git a/drivers/net/wireless/ath/ath9k/calib.c b/drivers/net/wireless/ath/ath9k/calib.c
index d8db74b0ef66..278365b8a895 100644
--- a/drivers/net/wireless/ath/ath9k/calib.c
+++ b/drivers/net/wireless/ath/ath9k/calib.c
@@ -63,13 +63,13 @@ static s16 ath9k_hw_get_default_nf(struct ath_hw *ah,
         return ath9k_hw_get_nf_limits(ah, chan)->nominal;
 }
 
-s16 ath9k_hw_getchan_noise(struct ath_hw *ah, struct ath9k_channel *chan)
+s16 ath9k_hw_getchan_noise(struct ath_hw *ah, struct ath9k_channel *chan,
+                           s16 nf)
 {
         s8 noise = ATH_DEFAULT_NOISE_FLOOR;
 
-        if (chan && chan->noisefloor) {
-                s8 delta = chan->noisefloor -
-                           ATH9K_NF_CAL_NOISE_THRESH -
+        if (nf) {
+                s8 delta = nf - ATH9K_NF_CAL_NOISE_THRESH -
                            ath9k_hw_get_default_nf(ah, chan);
                 if (delta > 0)
                         noise += delta;
@@ -392,7 +392,7 @@ bool ath9k_hw_getnf(struct ath_hw *ah, struct ath9k_channel *chan)
         clear_bit(NFCAL_PENDING, &caldata->cal_flags);
         ath9k_hw_update_nfcal_hist_buffer(ah, caldata, nfarray);
         chan->noisefloor = h[0].privNF;
-        ah->noise = ath9k_hw_getchan_noise(ah, chan);
+        ah->noise = ath9k_hw_getchan_noise(ah, chan, chan->noisefloor);
         return true;
 }
 EXPORT_SYMBOL(ath9k_hw_getnf);
diff --git a/drivers/net/wireless/ath/ath9k/calib.h b/drivers/net/wireless/ath/ath9k/calib.h
index 3d70b8c2bcdd..b8ed95e9a335 100644
--- a/drivers/net/wireless/ath/ath9k/calib.h
+++ b/drivers/net/wireless/ath/ath9k/calib.h
@@ -116,7 +116,8 @@ void ath9k_init_nfcal_hist_buffer(struct ath_hw *ah,
 void ath9k_hw_bstuck_nfcal(struct ath_hw *ah);
 void ath9k_hw_reset_calibration(struct ath_hw *ah,
                                 struct ath9k_cal_list *currCal);
-s16 ath9k_hw_getchan_noise(struct ath_hw *ah, struct ath9k_channel *chan);
+s16 ath9k_hw_getchan_noise(struct ath_hw *ah, struct ath9k_channel *chan,
+                           s16 nf);
 
 
 #endif /* CALIB_H */
diff --git a/drivers/net/wireless/ath/ath9k/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index 1be2c787aac9..83a2c59f680b 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -1050,6 +1050,9 @@ static ssize_t write_file_spec_scan_ctl(struct file *file,
         char buf[32];
         ssize_t len;
 
+        if (config_enabled(CONFIG_ATH9K_TX99))
+                return -EOPNOTSUPP;
+
         len = min(count, sizeof(buf) - 1);
         if (copy_from_user(buf, user_buf, len))
                 return -EFAULT;
@@ -1775,6 +1778,111 @@ void ath9k_deinit_debug(struct ath_softc *sc)
         }
 }
 
+static ssize_t read_file_tx99(struct file *file, char __user *user_buf,
+                              size_t count, loff_t *ppos)
+{
+        struct ath_softc *sc = file->private_data;
+        char buf[3];
+        unsigned int len;
+
+        len = sprintf(buf, "%d\n", sc->tx99_state);
+        return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+}
+
+static ssize_t write_file_tx99(struct file *file, const char __user *user_buf,
+                               size_t count, loff_t *ppos)
+{
+        struct ath_softc *sc = file->private_data;
+        struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+        char buf[32];
+        bool start;
+        ssize_t len;
+        int r;
+
+        if (sc->nvifs > 1)
+                return -EOPNOTSUPP;
+
+        len = min(count, sizeof(buf) - 1);
+        if (copy_from_user(buf, user_buf, len))
+                return -EFAULT;
+
+        if (strtobool(buf, &start))
+                return -EINVAL;
+
+        if (start == sc->tx99_state) {
+                if (!start)
+                        return count;
+                ath_dbg(common, XMIT, "Resetting TX99\n");
+                ath9k_tx99_deinit(sc);
+        }
+
+        if (!start) {
+                ath9k_tx99_deinit(sc);
+                return count;
+        }
+
+        r = ath9k_tx99_init(sc);
+        if (r)
+                return r;
+
+        return count;
+}
+
+static const struct file_operations fops_tx99 = {
+        .read = read_file_tx99,
+        .write = write_file_tx99,
+        .open = simple_open,
+        .owner = THIS_MODULE,
+        .llseek = default_llseek,
+};
+
+static ssize_t read_file_tx99_power(struct file *file,
+                                    char __user *user_buf,
+                                    size_t count, loff_t *ppos)
+{
+        struct ath_softc *sc = file->private_data;
+        char buf[32];
+        unsigned int len;
+
+        len = sprintf(buf, "%d (%d dBm)\n",
+                      sc->tx99_power,
+                      sc->tx99_power / 2);
+
+        return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+}
+
+static ssize_t write_file_tx99_power(struct file *file,
+                                     const char __user *user_buf,
+                                     size_t count, loff_t *ppos)
+{
+        struct ath_softc *sc = file->private_data;
+        int r;
+        u8 tx_power;
+
+        r = kstrtou8_from_user(user_buf, count, 0, &tx_power);
+        if (r)
+                return r;
+
+        if (tx_power > MAX_RATE_POWER)
+                return -EINVAL;
+
+        sc->tx99_power = tx_power;
+
+        ath9k_ps_wakeup(sc);
+        ath9k_hw_tx99_set_txpower(sc->sc_ah, sc->tx99_power);
+        ath9k_ps_restore(sc);
+
+        return count;
+}
+
+static const struct file_operations fops_tx99_power = {
+        .read = read_file_tx99_power,
+        .write = write_file_tx99_power,
+        .open = simple_open,
+        .owner = THIS_MODULE,
+        .llseek = default_llseek,
+};
+
 int ath9k_init_debug(struct ath_hw *ah)
 {
         struct ath_common *common = ath9k_hw_common(ah);
@@ -1866,5 +1974,15 @@ int ath9k_init_debug(struct ath_hw *ah)
         debugfs_create_file("btcoex", S_IRUSR, sc->debug.debugfs_phy, sc,
                             &fops_btcoex);
 #endif
+        if (config_enabled(CONFIG_ATH9K_TX99) &&
+            AR_SREV_9300_20_OR_LATER(ah)) {
+                debugfs_create_file("tx99", S_IRUSR | S_IWUSR,
+                                    sc->debug.debugfs_phy, sc,
+                                    &fops_tx99);
+                debugfs_create_file("tx99_power", S_IRUSR | S_IWUSR,
+                                    sc->debug.debugfs_phy, sc,
+                                    &fops_tx99_power);
+        }
+
         return 0;
 }
diff --git a/drivers/net/wireless/ath/ath9k/dfs.h b/drivers/net/wireless/ath/ath9k/dfs.h
index 3c839f06a06a..c6fa3d5b5d74 100644
--- a/drivers/net/wireless/ath/ath9k/dfs.h
+++ b/drivers/net/wireless/ath/ath9k/dfs.h
@@ -17,7 +17,7 @@
 
 #ifndef ATH9K_DFS_H
 #define ATH9K_DFS_H
-#include "dfs_pattern_detector.h"
+#include "../dfs_pattern_detector.h"
 
 #if defined(CONFIG_ATH9K_DFS_CERTIFIED)
 /**
diff --git a/drivers/net/wireless/ath/ath9k/dfs_debug.c b/drivers/net/wireless/ath/ath9k/dfs_debug.c
index 821599135d8a..90b8342d1ed4 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_debug.c
+++ b/drivers/net/wireless/ath/ath9k/dfs_debug.c
@@ -20,16 +20,16 @@
 
 #include "ath9k.h"
 #include "dfs_debug.h"
+#include "../dfs_pattern_detector.h"
 
-
-struct ath_dfs_pool_stats global_dfs_pool_stats = { 0 };
+static struct ath_dfs_pool_stats dfs_pool_stats = { 0 };
 
 #define ATH9K_DFS_STAT(s, p) \
         len += scnprintf(buf + len, size - len, "%28s : %10u\n", s, \
                          sc->debug.stats.dfs_stats.p);
 #define ATH9K_DFS_POOL_STAT(s, p) \
         len += scnprintf(buf + len, size - len, "%28s : %10u\n", s, \
-                         global_dfs_pool_stats.p);
+                         dfs_pool_stats.p);
 
 static ssize_t read_file_dfs(struct file *file, char __user *user_buf,
                              size_t count, loff_t *ppos)
@@ -44,6 +44,9 @@ static ssize_t read_file_dfs(struct file *file, char __user *user_buf,
         if (buf == NULL)
                 return -ENOMEM;
 
+        if (sc->dfs_detector)
+                dfs_pool_stats = sc->dfs_detector->get_stats(sc->dfs_detector);
+
         len += scnprintf(buf + len, size - len, "DFS support for "
                          "macVersion = 0x%x, macRev = 0x%x: %s\n",
                          hw_ver->macVersion, hw_ver->macRev,
diff --git a/drivers/net/wireless/ath/ath9k/dfs_debug.h b/drivers/net/wireless/ath/ath9k/dfs_debug.h
index e36810a4b585..0a7ddf4c88c9 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_debug.h
+++ b/drivers/net/wireless/ath/ath9k/dfs_debug.h
@@ -51,25 +51,11 @@ struct ath_dfs_stats {
         u32 radar_detected;
 };
 
-/**
- * struct ath_dfs_pool_stats - DFS Statistics for global pools
- */
-struct ath_dfs_pool_stats {
-        u32 pool_reference;
-        u32 pulse_allocated;
-        u32 pulse_alloc_error;
-        u32 pulse_used;
-        u32 pseq_allocated;
-        u32 pseq_alloc_error;
-        u32 pseq_used;
-};
 #if defined(CONFIG_ATH9K_DFS_DEBUGFS)
 
 #define DFS_STAT_INC(sc, c) (sc->debug.stats.dfs_stats.c++)
 void ath9k_dfs_init_debug(struct ath_softc *sc);
 
-#define DFS_POOL_STAT_INC(c) (global_dfs_pool_stats.c++)
-#define DFS_POOL_STAT_DEC(c) (global_dfs_pool_stats.c--)
 extern struct ath_dfs_pool_stats global_dfs_pool_stats;
 
 #else
@@ -77,8 +63,6 @@ extern struct ath_dfs_pool_stats global_dfs_pool_stats;
 #define DFS_STAT_INC(sc, c) do { } while (0)
 static inline void ath9k_dfs_init_debug(struct ath_softc *sc) { }
 
-#define DFS_POOL_STAT_INC(c) do { } while (0)
-#define DFS_POOL_STAT_DEC(c) do { } while (0)
 #endif /* CONFIG_ATH9K_DFS_DEBUGFS */
 
 #endif /* ATH9K_DFS_DEBUG_H */
diff --git a/drivers/net/wireless/ath/ath9k/hw-ops.h b/drivers/net/wireless/ath/ath9k/hw-ops.h
index 83f4927aeaca..4f9378ddf07f 100644
--- a/drivers/net/wireless/ath/ath9k/hw-ops.h
+++ b/drivers/net/wireless/ath/ath9k/hw-ops.h
@@ -78,6 +78,22 @@ static inline void ath9k_hw_antdiv_comb_conf_set(struct ath_hw *ah,
         ath9k_hw_ops(ah)->antdiv_comb_conf_set(ah, antconf);
 }
 
+static inline void ath9k_hw_tx99_start(struct ath_hw *ah, u32 qnum)
+{
+        ath9k_hw_ops(ah)->tx99_start(ah, qnum);
+}
+
+static inline void ath9k_hw_tx99_stop(struct ath_hw *ah)
+{
+        ath9k_hw_ops(ah)->tx99_stop(ah);
+}
+
+static inline void ath9k_hw_tx99_set_txpower(struct ath_hw *ah, u8 power)
+{
+        if (ath9k_hw_ops(ah)->tx99_set_txpower)
+                ath9k_hw_ops(ah)->tx99_set_txpower(ah, power);
+}
+
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
 
 static inline void ath9k_hw_set_bt_ant_diversity(struct ath_hw *ah, bool enable)
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index dcdbab48709e..54b04155e43b 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -1885,7 +1885,7 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
         } else if (caldata) {
                 clear_bit(PAPRD_PACKET_SENT, &caldata->cal_flags);
         }
-        ah->noise = ath9k_hw_getchan_noise(ah, chan);
+        ah->noise = ath9k_hw_getchan_noise(ah, chan, chan->noisefloor);
 
         if (fastcc) {
                 r = ath9k_hw_do_fastcc(ah, chan);
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index 81fcbc756122..9ea24f1cba73 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -703,6 +703,10 @@ struct ath_hw_ops {
         void (*spectral_scan_trigger)(struct ath_hw *ah);
         void (*spectral_scan_wait)(struct ath_hw *ah);
 
+        void (*tx99_start)(struct ath_hw *ah, u32 qnum);
+        void (*tx99_stop)(struct ath_hw *ah);
+        void (*tx99_set_txpower)(struct ath_hw *ah, u8 power);
+
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
         void (*set_bt_ant_diversity)(struct ath_hw *hw, bool enable);
 #endif
diff --git a/drivers/net/wireless/ath/ath9k/init.c b/drivers/net/wireless/ath/ath9k/init.c
index 7df728f36330..e89db64532f5 100644
--- a/drivers/net/wireless/ath/ath9k/init.c
+++ b/drivers/net/wireless/ath/ath9k/init.c
@@ -680,7 +680,9 @@ static int ath9k_init_softc(u16 devid, struct ath_softc *sc,
         sc->sc_ah = ah;
         pCap = &ah->caps;
 
-        sc->dfs_detector = dfs_pattern_detector_init(ah, NL80211_DFS_UNSET);
+        common = ath9k_hw_common(ah);
+        sc->dfs_detector = dfs_pattern_detector_init(common, NL80211_DFS_UNSET);
+        sc->tx99_power = MAX_RATE_POWER + 1;
 
         if (!pdata) {
                 ah->ah_flags |= AH_USE_EEPROM;
@@ -694,7 +696,6 @@ static int ath9k_init_softc(u16 devid, struct ath_softc *sc,
                 ah->external_reset = pdata->external_reset;
         }
 
-        common = ath9k_hw_common(ah);
         common->ops = &ah->reg_ops;
         common->bus_ops = bus_ops;
         common->ah = ah;
@@ -785,6 +786,7 @@ err_queues:
         ath9k_hw_deinit(ah);
 err_hw:
         ath9k_eeprom_release(sc);
+        dev_kfree_skb_any(sc->tx99_skb);
         return ret;
 }
 
@@ -842,7 +844,6 @@ static const struct ieee80211_iface_limit if_limits[] = {
                                  BIT(NL80211_IFTYPE_P2P_GO) },
 };
 
-
 static const struct ieee80211_iface_limit if_dfs_limits[] = {
         { .max = 1,     .types = BIT(NL80211_IFTYPE_AP) },
 };
@@ -903,17 +904,18 @@ void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 
         hw->wiphy->features |= NL80211_FEATURE_ACTIVE_MONITOR;
 
-        hw->wiphy->interface_modes =
-                BIT(NL80211_IFTYPE_P2P_GO) |
-                BIT(NL80211_IFTYPE_P2P_CLIENT) |
-                BIT(NL80211_IFTYPE_AP) |
-                BIT(NL80211_IFTYPE_WDS) |
-                BIT(NL80211_IFTYPE_STATION) |
-                BIT(NL80211_IFTYPE_ADHOC) |
-                BIT(NL80211_IFTYPE_MESH_POINT);
-
-        hw->wiphy->iface_combinations = if_comb;
-        hw->wiphy->n_iface_combinations = ARRAY_SIZE(if_comb);
+        if (!config_enabled(CONFIG_ATH9K_TX99)) {
+                hw->wiphy->interface_modes =
+                        BIT(NL80211_IFTYPE_P2P_GO) |
+                        BIT(NL80211_IFTYPE_P2P_CLIENT) |
+                        BIT(NL80211_IFTYPE_AP) |
+                        BIT(NL80211_IFTYPE_WDS) |
+                        BIT(NL80211_IFTYPE_STATION) |
+                        BIT(NL80211_IFTYPE_ADHOC) |
+                        BIT(NL80211_IFTYPE_MESH_POINT);
+                hw->wiphy->iface_combinations = if_comb;
+                hw->wiphy->n_iface_combinations = ARRAY_SIZE(if_comb);
+        }
 
         hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
diff --git a/drivers/net/wireless/ath/ath9k/link.c b/drivers/net/wireless/ath/ath9k/link.c
index 84a60644f93a..aed7e29dc50f 100644
--- a/drivers/net/wireless/ath/ath9k/link.c
+++ b/drivers/net/wireless/ath/ath9k/link.c
@@ -28,6 +28,13 @@ void ath_tx_complete_poll_work(struct work_struct *work)
         int i;
         bool needreset = false;
 
+
+        if (sc->tx99_state) {
+                ath_dbg(ath9k_hw_common(sc->sc_ah), RESET,
+                        "skip tx hung detection on tx99\n");
+                return;
+        }
+
         for (i = 0; i < IEEE80211_NUM_ACS; i++) {
                 txq = sc->tx.txq_map[i];
 
@@ -70,7 +77,7 @@ void ath_hw_check(struct work_struct *work)
         ath9k_ps_wakeup(sc);
         is_alive = ath9k_hw_check_alive(sc->sc_ah);
 
-        if (is_alive && !AR_SREV_9300(sc->sc_ah))
+        if ((is_alive && !AR_SREV_9300(sc->sc_ah)) || sc->tx99_state)
                 goto out;
         else if (!is_alive && AR_SREV_9300(sc->sc_ah)) {
                 ath_dbg(common, RESET,
@@ -141,6 +148,9 @@ void ath_hw_pll_work(struct work_struct *work)
         if (!test_bit(SC_OP_BEACONS, &sc->sc_flags))
                 return;
 
+        if (sc->tx99_state)
+                return;
+
         ath9k_ps_wakeup(sc);
         pll_sqsum = ar9003_get_pll_sqsum_dvc(sc->sc_ah);
         ath9k_ps_restore(sc);
@@ -518,7 +528,8 @@ void ath_update_survey_nf(struct ath_softc *sc, int channel)
 
         if (chan->noisefloor) {
                 survey->filled |= SURVEY_INFO_NOISE_DBM;
-                survey->noise = ath9k_hw_getchan_noise(ah, chan);
+                survey->noise = ath9k_hw_getchan_noise(ah, chan,
+                                                       chan->noisefloor);
         }
 }
 
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 20a2fbc1e34f..74f452c7b166 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -1046,6 +1046,14 @@ static int ath9k_add_interface(struct ieee80211_hw *hw,
 
         mutex_lock(&sc->mutex);
 
+        if (config_enabled(CONFIG_ATH9K_TX99)) {
+                if (sc->nvifs >= 1) {
+                        mutex_unlock(&sc->mutex);
+                        return -EOPNOTSUPP;
+                }
+                sc->tx99_vif = vif;
+        }
+
         ath_dbg(common, CONFIG, "Attach a VIF of type: %d\n", vif->type);
         sc->nvifs++;
 
@@ -1074,9 +1082,15 @@ static int ath9k_change_interface(struct ieee80211_hw *hw,
         struct ath_softc *sc = hw->priv;
         struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 
-        ath_dbg(common, CONFIG, "Change Interface\n");
         mutex_lock(&sc->mutex);
 
+        if (config_enabled(CONFIG_ATH9K_TX99)) {
+                mutex_unlock(&sc->mutex);
+                return -EOPNOTSUPP;
+        }
+
+        ath_dbg(common, CONFIG, "Change Interface\n");
+
         if (ath9k_uses_beacons(vif->type))
                 ath9k_beacon_remove_slot(sc, vif);
 
@@ -1106,6 +1120,7 @@ static void ath9k_remove_interface(struct ieee80211_hw *hw,
         mutex_lock(&sc->mutex);
 
         sc->nvifs--;
+        sc->tx99_vif = NULL;
 
         if (ath9k_uses_beacons(vif->type))
                 ath9k_beacon_remove_slot(sc, vif);
@@ -1127,6 +1142,9 @@ static void ath9k_enable_ps(struct ath_softc *sc)
         struct ath_hw *ah = sc->sc_ah;
         struct ath_common *common = ath9k_hw_common(ah);
 
+        if (config_enabled(CONFIG_ATH9K_TX99))
+                return;
+
         sc->ps_enabled = true;
         if (!(ah->caps.hw_caps & ATH9K_HW_CAP_AUTOSLEEP)) {
                 if ((ah->imask & ATH9K_INT_TIM_TIMER) == 0) {
@@ -1143,6 +1161,9 @@ static void ath9k_disable_ps(struct ath_softc *sc)
         struct ath_hw *ah = sc->sc_ah;
         struct ath_common *common = ath9k_hw_common(ah);
 
+        if (config_enabled(CONFIG_ATH9K_TX99))
+                return;
+
         sc->ps_enabled = false;
         ath9k_hw_setpower(ah, ATH9K_PM_AWAKE);
         if (!(ah->caps.hw_caps & ATH9K_HW_CAP_AUTOSLEEP)) {
@@ -1166,6 +1187,9 @@ void ath9k_spectral_scan_trigger(struct ieee80211_hw *hw)
         struct ath_common *common = ath9k_hw_common(ah);
         u32 rxfilter;
 
+        if (config_enabled(CONFIG_ATH9K_TX99))
+                return;
+
         if (!ath9k_hw_ops(ah)->spectral_scan_trigger) {
                 ath_err(common, "spectrum analyzer not implemented on this hardware\n");
                 return;
@@ -1745,6 +1769,9 @@ static int ath9k_get_survey(struct ieee80211_hw *hw, int idx,
         unsigned long flags;
         int pos;
 
+        if (config_enabled(CONFIG_ATH9K_TX99))
+                return -EOPNOTSUPP;
+
         spin_lock_irqsave(&common->cc_lock, flags);
         if (idx == 0)
                 ath_update_survey_stats(sc);
@@ -1777,6 +1804,9 @@ static void ath9k_set_coverage_class(struct ieee80211_hw *hw, u8 coverage_class)
         struct ath_softc *sc = hw->priv;
         struct ath_hw *ah = sc->sc_ah;
 
+        if (config_enabled(CONFIG_ATH9K_TX99))
+                return;
+
         mutex_lock(&sc->mutex);
         ah->coverage_class = coverage_class;
 
@@ -2343,6 +2373,134 @@ static void ath9k_channel_switch_beacon(struct ieee80211_hw *hw,
         sc->csa_vif = vif;
 }
 
+static void ath9k_tx99_stop(struct ath_softc *sc)
+{
+        struct ath_hw *ah = sc->sc_ah;
+        struct ath_common *common = ath9k_hw_common(ah);
+
+        ath_drain_all_txq(sc);
+        ath_startrecv(sc);
+
+        ath9k_hw_set_interrupts(ah);
+        ath9k_hw_enable_interrupts(ah);
+
+        ieee80211_wake_queues(sc->hw);
+
+        kfree_skb(sc->tx99_skb);
+        sc->tx99_skb = NULL;
+        sc->tx99_state = false;
+
+        ath9k_hw_tx99_stop(sc->sc_ah);
+        ath_dbg(common, XMIT, "TX99 stopped\n");
+}
+
+static struct sk_buff *ath9k_build_tx99_skb(struct ath_softc *sc)
+{
+        static u8 PN9Data[] = {0xff, 0x87, 0xb8, 0x59, 0xb7, 0xa1, 0xcc, 0x24,
+                               0x57, 0x5e, 0x4b, 0x9c, 0x0e, 0xe9, 0xea, 0x50,
+                               0x2a, 0xbe, 0xb4, 0x1b, 0xb6, 0xb0, 0x5d, 0xf1,
+                               0xe6, 0x9a, 0xe3, 0x45, 0xfd, 0x2c, 0x53, 0x18,
+                               0x0c, 0xca, 0xc9, 0xfb, 0x49, 0x37, 0xe5, 0xa8,
+                               0x51, 0x3b, 0x2f, 0x61, 0xaa, 0x72, 0x18, 0x84,
+                               0x02, 0x23, 0x23, 0xab, 0x63, 0x89, 0x51, 0xb3,
+                               0xe7, 0x8b, 0x72, 0x90, 0x4c, 0xe8, 0xfb, 0xc0};
+        u32 len = 1200;
+        struct ieee80211_hw *hw = sc->hw;
+        struct ieee80211_hdr *hdr;
+        struct ieee80211_tx_info *tx_info;
+        struct sk_buff *skb;
+
+        skb = alloc_skb(len, GFP_KERNEL);
+        if (!skb)
+                return NULL;
+
+        skb_put(skb, len);
+
+        memset(skb->data, 0, len);
+
+        hdr = (struct ieee80211_hdr *)skb->data;
+        hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA);
+        hdr->duration_id = 0;
+
+        memcpy(hdr->addr1, hw->wiphy->perm_addr, ETH_ALEN);
+        memcpy(hdr->addr2, hw->wiphy->perm_addr, ETH_ALEN);
+        memcpy(hdr->addr3, hw->wiphy->perm_addr, ETH_ALEN);
+
+        hdr->seq_ctrl |= cpu_to_le16(sc->tx.seq_no);
+
+        tx_info = IEEE80211_SKB_CB(skb);
+        memset(tx_info, 0, sizeof(*tx_info));
+        tx_info->band = hw->conf.chandef.chan->band;
+        tx_info->flags = IEEE80211_TX_CTL_NO_ACK;
+        tx_info->control.vif = sc->tx99_vif;
+
+        memcpy(skb->data + sizeof(*hdr), PN9Data, sizeof(PN9Data));
+
+        return skb;
+}
+
+void ath9k_tx99_deinit(struct ath_softc *sc)
+{
+        ath_reset(sc);
+
+        ath9k_ps_wakeup(sc);
+        ath9k_tx99_stop(sc);
+        ath9k_ps_restore(sc);
+}
+
+int ath9k_tx99_init(struct ath_softc *sc)
+{
+        struct ieee80211_hw *hw = sc->hw;
+        struct ath_hw *ah = sc->sc_ah;
+        struct ath_common *common = ath9k_hw_common(ah);
+        struct ath_tx_control txctl;
+        int r;
+
+        if (sc->sc_flags & SC_OP_INVALID) {
+                ath_err(common,
+                        "driver is in invalid state unable to use TX99");
+                return -EINVAL;
+        }
+
+        sc->tx99_skb = ath9k_build_tx99_skb(sc);
+        if (!sc->tx99_skb)
+                return -ENOMEM;
+
+        memset(&txctl, 0, sizeof(txctl));
+        txctl.txq = sc->tx.txq_map[IEEE80211_AC_VO];
+
+        ath_reset(sc);
+
+        ath9k_ps_wakeup(sc);
+
+        ath9k_hw_disable_interrupts(ah);
+        atomic_set(&ah->intr_ref_cnt, -1);
+        ath_drain_all_txq(sc);
+        ath_stoprecv(sc);
+
+        sc->tx99_state = true;
+
+        ieee80211_stop_queues(hw);
+
+        if (sc->tx99_power == MAX_RATE_POWER + 1)
+                sc->tx99_power = MAX_RATE_POWER;
+
+        ath9k_hw_tx99_set_txpower(ah, sc->tx99_power);
+        r = ath9k_tx99_send(sc, sc->tx99_skb, &txctl);
+        if (r) {
+                ath_dbg(common, XMIT, "Failed to xmit TX99 skb\n");
+                return r;
+        }
+
+        ath_dbg(common, XMIT, "TX99 xmit started using %d ( %ddBm)\n",
+                sc->tx99_power,
+                sc->tx99_power / 2);
+
+        /* We leave the harware awake as it will be chugging on */
+
+        return 0;
+}
+
 struct ieee80211_ops ath9k_ops = {
         .tx                 = ath9k_tx,
         .start              = ath9k_start,
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index 8b788efb41fd..95ddca5495d4 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -375,6 +375,9 @@ u32 ath_calcrxfilter(struct ath_softc *sc)
 {
         u32 rfilt;
 
+        if (config_enabled(CONFIG_ATH9K_TX99))
+                return 0;
+
         rfilt = ATH9K_RX_FILTER_UCAST | ATH9K_RX_FILTER_BCAST
                 | ATH9K_RX_FILTER_MCAST;
 
@@ -972,14 +975,15 @@ static int ath_process_fft(struct ath_softc *sc, struct ieee80211_hdr *hdr,
 {
 #ifdef CONFIG_ATH9K_DEBUGFS
         struct ath_hw *ah = sc->sc_ah;
-        u8 bins[SPECTRAL_HT20_NUM_BINS];
-        u8 *vdata = (u8 *)hdr;
-        struct fft_sample_ht20 fft_sample;
+        u8 num_bins, *bins, *vdata = (u8 *)hdr;
+        struct fft_sample_ht20 fft_sample_20;
+        struct fft_sample_ht20_40 fft_sample_40;
+        struct fft_sample_tlv *tlv;
         struct ath_radar_info *radar_info;
-        struct ath_ht20_mag_info *mag_info;
         int len = rs->rs_datalen;
         int dc_pos;
-        u16 length, max_magnitude;
+        u16 fft_len, length, freq = ah->curchan->chan->center_freq;
+        enum nl80211_channel_type chan_type;
 
         /* AR9280 and before report via ATH9K_PHYERR_RADAR, AR93xx and newer
          * via ATH9K_PHYERR_SPECTRAL. Haven't seen ATH9K_PHYERR_FALSE_RADAR_EXT
@@ -997,45 +1001,44 @@ static int ath_process_fft(struct ath_softc *sc, struct ieee80211_hdr *hdr,
         if (!(radar_info->pulse_bw_info & SPECTRAL_SCAN_BITMASK))
                 return 0;
 
-        /* Variation in the data length is possible and will be fixed later.
-         * Note that we only support HT20 for now.
-         *
-         * TODO: add HT20_40 support as well.
-         */
-        if ((len > SPECTRAL_HT20_TOTAL_DATA_LEN + 2) ||
-            (len < SPECTRAL_HT20_TOTAL_DATA_LEN - 1))
-                return 1;
-
-        fft_sample.tlv.type = ATH_FFT_SAMPLE_HT20;
-        length = sizeof(fft_sample) - sizeof(fft_sample.tlv);
-        fft_sample.tlv.length = __cpu_to_be16(length);
+        chan_type = cfg80211_get_chandef_type(&sc->hw->conf.chandef);
+        if ((chan_type == NL80211_CHAN_HT40MINUS) ||
+            (chan_type == NL80211_CHAN_HT40PLUS)) {
+                fft_len = SPECTRAL_HT20_40_TOTAL_DATA_LEN;
+                num_bins = SPECTRAL_HT20_40_NUM_BINS;
+                bins = (u8 *)fft_sample_40.data;
+        } else {
+                fft_len = SPECTRAL_HT20_TOTAL_DATA_LEN;
+                num_bins = SPECTRAL_HT20_NUM_BINS;
+                bins = (u8 *)fft_sample_20.data;
+        }
 
-        fft_sample.freq = __cpu_to_be16(ah->curchan->chan->center_freq);
-        fft_sample.rssi = fix_rssi_inv_only(rs->rs_rssi_ctl0);
-        fft_sample.noise = ah->noise;
+        /* Variation in the data length is possible and will be fixed later */
+        if ((len > fft_len + 2) || (len < fft_len - 1))
+                return 1;
 
-        switch (len - SPECTRAL_HT20_TOTAL_DATA_LEN) {
+        switch (len - fft_len) {
         case 0:
                 /* length correct, nothing to do. */
-                memcpy(bins, vdata, SPECTRAL_HT20_NUM_BINS);
+                memcpy(bins, vdata, num_bins);
                 break;
         case -1:
                 /* first byte missing, duplicate it. */
-                memcpy(&bins[1], vdata, SPECTRAL_HT20_NUM_BINS - 1);
+                memcpy(&bins[1], vdata, num_bins - 1);
                 bins[0] = vdata[0];
                 break;
         case 2:
                 /* MAC added 2 extra bytes at bin 30 and 32, remove them. */
                 memcpy(bins, vdata, 30);
                 bins[30] = vdata[31];
-                memcpy(&bins[31], &vdata[33], SPECTRAL_HT20_NUM_BINS - 31);
+                memcpy(&bins[31], &vdata[33], num_bins - 31);
                 break;
         case 1:
                 /* MAC added 2 extra bytes AND first byte is missing. */
                 bins[0] = vdata[0];
-                memcpy(&bins[0], vdata, 30);
+                memcpy(&bins[1], vdata, 30);
                 bins[31] = vdata[31];
-                memcpy(&bins[32], &vdata[33], SPECTRAL_HT20_NUM_BINS - 32);
+                memcpy(&bins[32], &vdata[33], num_bins - 32);
                 break;
         default:
                 return 1;
@@ -1044,23 +1047,93 @@ static int ath_process_fft(struct ath_softc *sc, struct ieee80211_hdr *hdr,
         /* DC value (value in the middle) is the blind spot of the spectral
          * sample and invalid, interpolate it.
          */
-        dc_pos = SPECTRAL_HT20_NUM_BINS / 2;
+        dc_pos = num_bins / 2;
         bins[dc_pos] = (bins[dc_pos + 1] + bins[dc_pos - 1]) / 2;
 
-        /* mag data is at the end of the frame, in front of radar_info */
-        mag_info = ((struct ath_ht20_mag_info *)radar_info) - 1;
+        if ((chan_type == NL80211_CHAN_HT40MINUS) ||
+            (chan_type == NL80211_CHAN_HT40PLUS)) {
+                s8 lower_rssi, upper_rssi;
+                s16 ext_nf;
+                u8 lower_max_index, upper_max_index;
+                u8 lower_bitmap_w, upper_bitmap_w;
+                u16 lower_mag, upper_mag;
+                struct ath9k_hw_cal_data *caldata = ah->caldata;
+                struct ath_ht20_40_mag_info *mag_info;
+
+                if (caldata)
+                        ext_nf = ath9k_hw_getchan_noise(ah, ah->curchan,
+                                        caldata->nfCalHist[3].privNF);
+                else
+                        ext_nf = ATH_DEFAULT_NOISE_FLOOR;
+
+                length = sizeof(fft_sample_40) - sizeof(struct fft_sample_tlv);
+                fft_sample_40.tlv.type = ATH_FFT_SAMPLE_HT20_40;
+                fft_sample_40.tlv.length = __cpu_to_be16(length);
+                fft_sample_40.freq = __cpu_to_be16(freq);
+                fft_sample_40.channel_type = chan_type;
+
+                if (chan_type == NL80211_CHAN_HT40PLUS) {
+                        lower_rssi = fix_rssi_inv_only(rs->rs_rssi_ctl0);
+                        upper_rssi = fix_rssi_inv_only(rs->rs_rssi_ext0);
 
-        /* copy raw bins without scaling them */
-        memcpy(fft_sample.data, bins, SPECTRAL_HT20_NUM_BINS);
-        fft_sample.max_exp = mag_info->max_exp & 0xf;
+                        fft_sample_40.lower_noise = ah->noise;
+                        fft_sample_40.upper_noise = ext_nf;
+                } else {
+                        lower_rssi = fix_rssi_inv_only(rs->rs_rssi_ext0);
+                        upper_rssi = fix_rssi_inv_only(rs->rs_rssi_ctl0);
 
-        max_magnitude = spectral_max_magnitude(mag_info->all_bins);
-        fft_sample.max_magnitude = __cpu_to_be16(max_magnitude);
-        fft_sample.max_index = spectral_max_index(mag_info->all_bins);
-        fft_sample.bitmap_weight = spectral_bitmap_weight(mag_info->all_bins);
-        fft_sample.tsf = __cpu_to_be64(tsf);
+                        fft_sample_40.lower_noise = ext_nf;
+                        fft_sample_40.upper_noise = ah->noise;
+                }
+                fft_sample_40.lower_rssi = lower_rssi;
+                fft_sample_40.upper_rssi = upper_rssi;
+
+                mag_info = ((struct ath_ht20_40_mag_info *)radar_info) - 1;
+                lower_mag = spectral_max_magnitude(mag_info->lower_bins);
+                upper_mag = spectral_max_magnitude(mag_info->upper_bins);
+                fft_sample_40.lower_max_magnitude = __cpu_to_be16(lower_mag);
+                fft_sample_40.upper_max_magnitude = __cpu_to_be16(upper_mag);
+                lower_max_index = spectral_max_index(mag_info->lower_bins);
+                upper_max_index = spectral_max_index(mag_info->upper_bins);
+                fft_sample_40.lower_max_index = lower_max_index;
+                fft_sample_40.upper_max_index = upper_max_index;
+                lower_bitmap_w = spectral_bitmap_weight(mag_info->lower_bins);
+                upper_bitmap_w = spectral_bitmap_weight(mag_info->upper_bins);
+                fft_sample_40.lower_bitmap_weight = lower_bitmap_w;
+                fft_sample_40.upper_bitmap_weight = upper_bitmap_w;
+                fft_sample_40.max_exp = mag_info->max_exp & 0xf;
+
+                fft_sample_40.tsf = __cpu_to_be64(tsf);
+
+                tlv = (struct fft_sample_tlv *)&fft_sample_40;
+        } else {
+                u8 max_index, bitmap_w;
+                u16 magnitude;
+                struct ath_ht20_mag_info *mag_info;
+
+                length = sizeof(fft_sample_20) - sizeof(struct fft_sample_tlv);
+                fft_sample_20.tlv.type = ATH_FFT_SAMPLE_HT20;
+                fft_sample_20.tlv.length = __cpu_to_be16(length);
+                fft_sample_20.freq = __cpu_to_be16(freq);
+
+                fft_sample_20.rssi = fix_rssi_inv_only(rs->rs_rssi_ctl0);
+                fft_sample_20.noise = ah->noise;
+
+                mag_info = ((struct ath_ht20_mag_info *)radar_info) - 1;
+                magnitude = spectral_max_magnitude(mag_info->all_bins);
+                fft_sample_20.max_magnitude = __cpu_to_be16(magnitude);
+                max_index = spectral_max_index(mag_info->all_bins);
+                fft_sample_20.max_index = max_index;
+                bitmap_w = spectral_bitmap_weight(mag_info->all_bins);
+                fft_sample_20.bitmap_weight = bitmap_w;
+                fft_sample_20.max_exp = mag_info->max_exp & 0xf;
+
+                fft_sample_20.tsf = __cpu_to_be64(tsf);
+
+                tlv = (struct fft_sample_tlv *)&fft_sample_20;
+        }
 
-        ath_debug_send_fft_sample(sc, &fft_sample.tlv);
+        ath_debug_send_fft_sample(sc, tlv);
         return 1;
 #else
         return 0;
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 47696d29743c..09cdbcd09739 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -1241,12 +1241,13 @@ static void ath_tx_fill_desc(struct ath_softc *sc, struct ath_buf *bf,
                 if (bf->bf_next)
                         info.link = bf->bf_next->bf_daddr;
                 else
-                        info.link = 0;
+                        info.link = (sc->tx99_state) ? bf->bf_daddr : 0;
 
                 if (!bf_first) {
                         bf_first = bf;
 
-                        info.flags = ATH9K_TXDESC_INTREQ;
+                        if (!sc->tx99_state)
+                                info.flags = ATH9K_TXDESC_INTREQ;
                         if ((tx_info->flags & IEEE80211_TX_CTL_CLEAR_PS_FILT) ||
                             txq == sc->tx.uapsdq)
                                 info.flags |= ATH9K_TXDESC_CLRDMASK;
@@ -1941,7 +1942,7 @@ static void ath_tx_txqaddbuf(struct ath_softc *sc, struct ath_txq *txq,
                         txq->axq_qnum, ito64(bf->bf_daddr), bf->bf_desc);
         }
 
-        if (!edma) {
+        if (!edma || sc->tx99_state) {
                 TX_STAT_INC(txq->axq_qnum, txstart);
                 ath9k_hw_txstart(ah, txq->axq_qnum);
         }
@@ -2020,6 +2021,9 @@ static void setup_frame_info(struct ieee80211_hw *hw,
                 fi->keyix = ATH9K_TXKEYIX_INVALID;
         fi->keytype = keytype;
         fi->framelen = framelen;
+
+        if (!rate)
+                return;
         fi->rtscts_rate = rate->hw_value;
         if (short_preamble)
                 fi->rtscts_rate |= rate->hw_value_short;
@@ -2371,6 +2375,8 @@ static void ath_tx_complete_buf(struct ath_softc *sc, struct ath_buf *bf,
 
         dma_unmap_single(sc->dev, bf->bf_buf_addr, skb->len, DMA_TO_DEVICE);
         bf->bf_buf_addr = 0;
+        if (sc->tx99_state)
+                goto skip_tx_complete;
 
         if (bf->bf_state.bfs_paprd) {
                 if (time_after(jiffies,
@@ -2383,6 +2389,7 @@ static void ath_tx_complete_buf(struct ath_softc *sc, struct ath_buf *bf,
                 ath_debug_stat_tx(sc, bf, ts, txq, tx_flags);
                 ath_tx_complete(sc, skb, tx_flags, txq);
         }
+skip_tx_complete:
         /* At this point, skb (bf->bf_mpdu) is consumed...make sure we don't
          * accidentally reference it later.
          */
@@ -2741,3 +2748,46 @@ void ath_tx_node_cleanup(struct ath_softc *sc, struct ath_node *an)
                 ath_txq_unlock(sc, txq);
         }
 }
+
+int ath9k_tx99_send(struct ath_softc *sc, struct sk_buff *skb,
+                    struct ath_tx_control *txctl)
+{
+        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+        struct ath_frame_info *fi = get_frame_info(skb);
+        struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+        struct ath_buf *bf;
+        int padpos, padsize;
+
+        padpos = ieee80211_hdrlen(hdr->frame_control);
+        padsize = padpos & 3;
+
+        if (padsize && skb->len > padpos) {
+                if (skb_headroom(skb) < padsize) {
+                        ath_dbg(common, XMIT,
+                                "tx99 padding failed\n");
+                return -EINVAL;
+                }
+
+                skb_push(skb, padsize);
+                memmove(skb->data, skb->data + padsize, padpos);
+        }
+
+        fi->keyix = ATH9K_TXKEYIX_INVALID;
+        fi->framelen = skb->len + FCS_LEN;
+        fi->keytype = ATH9K_KEY_TYPE_CLEAR;
+
+        bf = ath_tx_setup_buffer(sc, txctl->txq, NULL, skb);
+        if (!bf) {
+                ath_dbg(common, XMIT, "tx99 buffer setup failed\n");
+                return -EINVAL;
+        }
+
+        ath_set_rates(sc->tx99_vif, NULL, bf);
+
+        ath9k_hw_set_desc_link(sc->sc_ah, bf->bf_desc, bf->bf_daddr);
+        ath9k_hw_tx99_start(sc->sc_ah, txctl->txq->axq_qnum);
+
+        ath_tx_send_normal(sc, txctl->txq, NULL, skb);
+
+        return 0;
+}
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c b/drivers/net/wireless/ath/dfs_pattern_detector.c
index 491305c81fce..a1a69c5db409 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.c
+++ b/drivers/net/wireless/ath/dfs_pattern_detector.c
@@ -19,7 +19,7 @@
 
 #include "dfs_pattern_detector.h"
 #include "dfs_pri_detector.h"
-#include "ath9k.h"
+#include "ath.h"
 
 /*
  * tolerated deviation of radar time stamp in usecs on both sides
@@ -143,7 +143,6 @@ channel_detector_create(struct dfs_pattern_detector *dpd, u16 freq)
 {
         u32 sz, i;
         struct channel_detector *cd;
-        struct ath_common *common = ath9k_hw_common(dpd->ah);
 
         cd = kmalloc(sizeof(*cd), GFP_ATOMIC);
         if (cd == NULL)
@@ -167,7 +166,7 @@ channel_detector_create(struct dfs_pattern_detector *dpd, u16 freq)
         return cd;
 
 fail:
-        ath_dbg(common, DFS,
+        ath_dbg(dpd->common, DFS,
                 "failed to allocate channel_detector for freq=%d\n", freq);
         channel_detector_exit(dpd, cd);
         return NULL;
@@ -242,7 +241,7 @@ dpd_add_pulse(struct dfs_pattern_detector *dpd, struct pulse_event *event)
                 struct pri_detector *pd = cd->detectors[i];
                 struct pri_sequence *ps = pd->add_pulse(pd, event);
                 if (ps != NULL) {
-                        ath_dbg(ath9k_hw_common(dpd->ah), DFS,
+                        ath_dbg(dpd->common, DFS,
                                 "DFS: radar found on freq=%d: id=%d, pri=%d, "
                                 "count=%d, count_false=%d\n",
                                 event->freq, pd->rs->type_id,
@@ -254,6 +253,12 @@ dpd_add_pulse(struct dfs_pattern_detector *dpd, struct pulse_event *event)
         return false;
 }
 
+static struct ath_dfs_pool_stats
+dpd_get_stats(struct dfs_pattern_detector *dpd)
+{
+        return global_dfs_pool_stats;
+}
+
 static bool dpd_set_domain(struct dfs_pattern_detector *dpd,
                            enum nl80211_dfs_regions region)
 {
@@ -284,14 +289,18 @@ static struct dfs_pattern_detector default_dpd = {
         .exit           = dpd_exit,
         .set_dfs_domain = dpd_set_domain,
         .add_pulse      = dpd_add_pulse,
+        .get_stats      = dpd_get_stats,
         .region         = NL80211_DFS_UNSET,
 };
 
 struct dfs_pattern_detector *
-dfs_pattern_detector_init(struct ath_hw *ah, enum nl80211_dfs_regions region)
+dfs_pattern_detector_init(struct ath_common *common,
+                          enum nl80211_dfs_regions region)
 {
         struct dfs_pattern_detector *dpd;
-        struct ath_common *common = ath9k_hw_common(ah);
+
+        if (!config_enabled(CONFIG_CFG80211_CERTIFICATION_ONUS))
+                return NULL;
 
         dpd = kmalloc(sizeof(*dpd), GFP_KERNEL);
         if (dpd == NULL)
@@ -300,7 +309,7 @@ dfs_pattern_detector_init(struct ath_hw *ah, enum nl80211_dfs_regions region)
         *dpd = default_dpd;
         INIT_LIST_HEAD(&dpd->channel_detectors);
 
-        dpd->ah = ah;
+        dpd->common = common;
         if (dpd->set_dfs_domain(dpd, region))
                 return dpd;
 
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.h b/drivers/net/wireless/ath/dfs_pattern_detector.h
index 90a5abcc4265..dde2652b787c 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_pattern_detector.h
+++ b/drivers/net/wireless/ath/dfs_pattern_detector.h
@@ -22,6 +22,19 @@
 #include <linux/nl80211.h>
 
 /**
+ * struct ath_dfs_pool_stats - DFS Statistics for global pools
+ */
+struct ath_dfs_pool_stats {
+        u32 pool_reference;
+        u32 pulse_allocated;
+        u32 pulse_alloc_error;
+        u32 pulse_used;
+        u32 pseq_allocated;
+        u32 pseq_alloc_error;
+        u32 pseq_used;
+};
+
+/**
  * struct pulse_event - describing pulses reported by PHY
  * @ts: pulse time stamp in us
  * @freq: channel frequency in MHz
@@ -77,11 +90,12 @@ struct dfs_pattern_detector {
         bool (*add_pulse)(struct dfs_pattern_detector *dpd,
                           struct pulse_event *pe);
 
+        struct ath_dfs_pool_stats (*get_stats)(struct dfs_pattern_detector *dpd);
         enum nl80211_dfs_regions region;
         u8 num_radar_types;
         u64 last_pulse_ts;
         /* needed for ath_dbg() */
-        struct ath_hw *ah;
+        struct ath_common *common;
 
         const struct radar_detector_specs *radar_spec;
         struct list_head channel_detectors;
@@ -92,15 +106,7 @@ struct dfs_pattern_detector {
  * @param region: DFS domain to be used, can be NL80211_DFS_UNSET at creation
  * @return instance pointer on success, NULL otherwise
  */
-#if defined(CONFIG_ATH9K_DFS_CERTIFIED)
 extern struct dfs_pattern_detector *
-dfs_pattern_detector_init(struct ath_hw *ah, enum nl80211_dfs_regions region);
-#else
-static inline struct dfs_pattern_detector *
-dfs_pattern_detector_init(struct ath_hw *ah, enum nl80211_dfs_regions region)
-{
-        return NULL;
-}
-#endif /* CONFIG_ATH9K_DFS_CERTIFIED */
-
+dfs_pattern_detector_init(struct ath_common *common,
+                          enum nl80211_dfs_regions region);
 #endif /* DFS_PATTERN_DETECTOR_H */
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c b/drivers/net/wireless/ath/dfs_pri_detector.c
index c718fc379a10..43b608178884 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_pri_detector.c
+++ b/drivers/net/wireless/ath/dfs_pri_detector.c
@@ -17,10 +17,14 @@
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 
-#include "ath9k.h"
+#include "ath.h"
 #include "dfs_pattern_detector.h"
 #include "dfs_pri_detector.h"
-#include "dfs_debug.h"
+
+struct ath_dfs_pool_stats global_dfs_pool_stats = {};
+
+#define DFS_POOL_STAT_INC(c) (global_dfs_pool_stats.c++)
+#define DFS_POOL_STAT_DEC(c) (global_dfs_pool_stats.c--)
 
 /**
  * struct pulse_elem - elements in pulse queue
diff --git a/drivers/net/wireless/ath/ath9k/dfs_pri_detector.h b/drivers/net/wireless/ath/dfs_pri_detector.h
index 723962d1abc6..79f0fff4d1e6 100644
--- a/drivers/net/wireless/ath/ath9k/dfs_pri_detector.h
+++ b/drivers/net/wireless/ath/dfs_pri_detector.h
@@ -19,6 +19,8 @@
 
 #include <linux/list.h>
 
+extern struct ath_dfs_pool_stats global_dfs_pool_stats;
+
 /**
  * struct pri_sequence - sequence of pulses matching one PRI
  * @head: list_head
diff --git a/drivers/net/wireless/ath/regd.c b/drivers/net/wireless/ath/regd.c
index 7d077c752dd5..c00687e05688 100644
--- a/drivers/net/wireless/ath/regd.c
+++ b/drivers/net/wireless/ath/regd.c
@@ -356,14 +356,131 @@ static u16 ath_regd_find_country_by_name(char *alpha2)
         return -1;
 }
 
+static int __ath_reg_dyn_country(struct wiphy *wiphy,
+                                 struct ath_regulatory *reg,
+                                 struct regulatory_request *request)
+{
+        u16 country_code;
+
+        if (!ath_is_world_regd(reg))
+                return -EINVAL;
+
+        country_code = ath_regd_find_country_by_name(request->alpha2);
+        if (country_code == (u16) -1)
+                return -EINVAL;
+
+        reg->current_rd = COUNTRY_ERD_FLAG;
+        reg->current_rd |= country_code;
+
+        __ath_regd_init(reg);
+
+        ath_reg_apply_world_flags(wiphy, request->initiator, reg);
+
+        return 0;
+}
+
+static void ath_reg_dyn_country(struct wiphy *wiphy,
+                                struct ath_regulatory *reg,
+                                struct regulatory_request *request)
+{
+        if (__ath_reg_dyn_country(wiphy, reg, request))
+                return;
+
+        printk(KERN_DEBUG "ath: regdomain 0x%0x "
+                          "dynamically updated by %s\n",
+               reg->current_rd,
+               reg_initiator_name(request->initiator));
+}
+
+static bool dynamic_country_user_possible(struct ath_regulatory *reg)
+{
+        if (config_enabled(CONFIG_ATH_REG_DYNAMIC_USER_CERT_TESTING))
+                return true;
+
+        switch (reg->country_code) {
+        case CTRY_UNITED_STATES:
+        case CTRY_JAPAN1:
+        case CTRY_JAPAN2:
+        case CTRY_JAPAN3:
+        case CTRY_JAPAN4:
+        case CTRY_JAPAN5:
+        case CTRY_JAPAN6:
+        case CTRY_JAPAN7:
+        case CTRY_JAPAN8:
+        case CTRY_JAPAN9:
+        case CTRY_JAPAN10:
+        case CTRY_JAPAN11:
+        case CTRY_JAPAN12:
+        case CTRY_JAPAN13:
+        case CTRY_JAPAN14:
+        case CTRY_JAPAN15:
+        case CTRY_JAPAN16:
+        case CTRY_JAPAN17:
+        case CTRY_JAPAN18:
+        case CTRY_JAPAN19:
+        case CTRY_JAPAN20:
+        case CTRY_JAPAN21:
+        case CTRY_JAPAN22:
+        case CTRY_JAPAN23:
+        case CTRY_JAPAN24:
+        case CTRY_JAPAN25:
+        case CTRY_JAPAN26:
+        case CTRY_JAPAN27:
+        case CTRY_JAPAN28:
+        case CTRY_JAPAN29:
+        case CTRY_JAPAN30:
+        case CTRY_JAPAN31:
+        case CTRY_JAPAN32:
+        case CTRY_JAPAN33:
+        case CTRY_JAPAN34:
+        case CTRY_JAPAN35:
+        case CTRY_JAPAN36:
+        case CTRY_JAPAN37:
+        case CTRY_JAPAN38:
+        case CTRY_JAPAN39:
+        case CTRY_JAPAN40:
+        case CTRY_JAPAN41:
+        case CTRY_JAPAN42:
+        case CTRY_JAPAN43:
+        case CTRY_JAPAN44:
+        case CTRY_JAPAN45:
+        case CTRY_JAPAN46:
+        case CTRY_JAPAN47:
+        case CTRY_JAPAN48:
+        case CTRY_JAPAN49:
+        case CTRY_JAPAN50:
+        case CTRY_JAPAN51:
+        case CTRY_JAPAN52:
+        case CTRY_JAPAN53:
+        case CTRY_JAPAN54:
+        case CTRY_JAPAN55:
+        case CTRY_JAPAN56:
+        case CTRY_JAPAN57:
+        case CTRY_JAPAN58:
+        case CTRY_JAPAN59:
+                return false;
+        }
+
+        return true;
+}
+
+static void ath_reg_dyn_country_user(struct wiphy *wiphy,
+                                     struct ath_regulatory *reg,
+                                     struct regulatory_request *request)
+{
+        if (!config_enabled(CONFIG_ATH_REG_DYNAMIC_USER_REG_HINTS))
+                return;
+        if (!dynamic_country_user_possible(reg))
+                return;
+        ath_reg_dyn_country(wiphy, reg, request);
+}
+
 void ath_reg_notifier_apply(struct wiphy *wiphy,
                             struct regulatory_request *request,
                             struct ath_regulatory *reg)
 {
         struct ath_common *common = container_of(reg, struct ath_common,
                                                  regulatory);
-        u16 country_code;
-
         /* We always apply this */
         ath_reg_apply_radar_flags(wiphy);
 
@@ -388,25 +505,12 @@ void ath_reg_notifier_apply(struct wiphy *wiphy,
                        sizeof(struct ath_regulatory));
                 break;
         case NL80211_REGDOM_SET_BY_DRIVER:
+                break;
         case NL80211_REGDOM_SET_BY_USER:
+                ath_reg_dyn_country_user(wiphy, reg, request);
                 break;
         case NL80211_REGDOM_SET_BY_COUNTRY_IE:
-                if (!ath_is_world_regd(reg))
-                        break;
-
-                country_code = ath_regd_find_country_by_name(request->alpha2);
-                if (country_code == (u16) -1)
-                        break;
-
-                reg->current_rd = COUNTRY_ERD_FLAG;
-                reg->current_rd |= country_code;
-
-                printk(KERN_DEBUG "ath: regdomain 0x%0x updated by CountryIE\n",
-                        reg->current_rd);
-                __ath_regd_init(reg);
-
-                ath_reg_apply_world_flags(wiphy, request->initiator, reg);
-
+                ath_reg_dyn_country(wiphy, reg, request);
                 break;
         }
 }
diff --git a/drivers/net/wireless/atmel.c b/drivers/net/wireless/atmel.c
index a55ae6494c3b..0d950f209dae 100644
--- a/drivers/net/wireless/atmel.c
+++ b/drivers/net/wireless/atmel.c
@@ -3212,7 +3212,7 @@ static void associate(struct atmel_private *priv, u16 frame_len, u16 subtype)
         if (subtype == IEEE80211_STYPE_REASSOC_RESP &&
             status != WLAN_STATUS_ASSOC_DENIED_RATES &&
             status != WLAN_STATUS_CAPS_UNSUPPORTED &&
-            priv->AssociationRequestRetryCnt < MAX_ASSOCIATION_RETRIES) {
+            priv->ReAssociationRequestRetryCnt < MAX_ASSOCIATION_RETRIES) {
                 mod_timer(&priv->management_timer, jiffies + MGMT_JIFFIES);
                 priv->ReAssociationRequestRetryCnt++;
                 send_association_request(priv, 1);
diff --git a/drivers/net/wireless/b43/phy_n.c b/drivers/net/wireless/b43/phy_n.c
index 7c970d3ae358..05ee7f10cc8f 100644
--- a/drivers/net/wireless/b43/phy_n.c
+++ b/drivers/net/wireless/b43/phy_n.c
@@ -164,7 +164,8 @@ static void b43_nphy_rf_ctl_override_rev7(struct b43_wldev *dev, u16 field,
                 }
                 en_addr = en_addrs[override][i];
 
-                val_addr = (i == 0) ? e->val_addr_core0 : e->val_addr_core1;
+                if (e)
+                        val_addr = (i == 0) ? e->val_addr_core0 : e->val_addr_core1;
 
                 if (off) {
                         b43_phy_mask(dev, en_addr, ~en_mask);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
index e13b1a65c65f..3e10b801eee8 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
@@ -26,7 +26,6 @@
 #include <linux/mmc/sdio.h>
 #include <linux/mmc/sdio_func.h>
 #include <linux/mmc/card.h>
-#include <linux/mmc/host.h>
 #include <linux/platform_data/brcmfmac-sdio.h>
 
 #include <defs.h>
@@ -239,7 +238,9 @@ brcmf_sdio_regrw_helper(struct brcmf_sdio_dev *sdiodev, u32 addr,
                 func_num = SDIO_FUNC_1;
                 reg_size = 4;
 
-                brcmf_sdio_addrprep(sdiodev, reg_size, &addr);
+                ret = brcmf_sdio_addrprep(sdiodev, reg_size, &addr);
+                if (ret)
+                        goto done;
         }
 
         do {
@@ -255,6 +256,7 @@ brcmf_sdio_regrw_helper(struct brcmf_sdio_dev *sdiodev, u32 addr,
                                                        func_num, addr, data, 4);
         } while (ret != 0 && retry++ < SDIOH_API_ACCESS_RETRY_LIMIT);
 
+done:
         if (ret != 0)
                 brcmf_err("failed with %d\n", ret);
 
@@ -315,8 +317,36 @@ void brcmf_sdio_regwl(struct brcmf_sdio_dev *sdiodev, u32 addr,
                 *ret = retval;
 }
 
+static int brcmf_sdio_buffrw(struct brcmf_sdio_dev *sdiodev, uint fn,
+                             bool write, u32 addr, struct sk_buff *pkt)
+{
+        unsigned int req_sz;
+
+        brcmf_pm_resume_wait(sdiodev, &sdiodev->request_buffer_wait);
+        if (brcmf_pm_resume_error(sdiodev))
+                return -EIO;
+
+        /* Single skb use the standard mmc interface */
+        req_sz = pkt->len + 3;
+        req_sz &= (uint)~3;
+
+        if (write)
+                return sdio_memcpy_toio(sdiodev->func[fn], addr,
+                                        ((u8 *)(pkt->data)),
+                                        req_sz);
+        else if (fn == 1)
+                return sdio_memcpy_fromio(sdiodev->func[fn],
+                                          ((u8 *)(pkt->data)),
+                                          addr, req_sz);
+        else
+                /* function 2 read is FIFO operation */
+                return sdio_readsb(sdiodev->func[fn],
+                                   ((u8 *)(pkt->data)), addr,
+                                   req_sz);
+}
+
 /**
- * brcmf_sdio_buffrw - SDIO interface function for block data access
+ * brcmf_sdio_sglist_rw - SDIO interface function for block data access
  * @sdiodev: brcmfmac sdio device
  * @fn: SDIO function number
  * @write: direction flag
@@ -327,12 +357,13 @@ void brcmf_sdio_regwl(struct brcmf_sdio_dev *sdiodev, u32 addr,
  * stack for block data access. It assumes that the skb passed down by the
  * caller has already been padded and aligned.
  */
-static int brcmf_sdio_buffrw(struct brcmf_sdio_dev *sdiodev, uint fn,
-                             bool write, u32 addr, struct sk_buff_head *pktlist)
+static int brcmf_sdio_sglist_rw(struct brcmf_sdio_dev *sdiodev, uint fn,
+                                bool write, u32 addr,
+                                struct sk_buff_head *pktlist)
 {
         unsigned int req_sz, func_blk_sz, sg_cnt, sg_data_sz, pkt_offset;
-        unsigned int max_blks, max_req_sz, orig_offset, dst_offset;
-        unsigned short max_seg_sz, seg_sz;
+        unsigned int max_req_sz, orig_offset, dst_offset;
+        unsigned short max_seg_cnt, seg_sz;
         unsigned char *pkt_data, *orig_data, *dst_data;
         struct sk_buff *pkt_next = NULL, *local_pkt_next;
         struct sk_buff_head local_list, *target_list;
@@ -341,7 +372,6 @@ static int brcmf_sdio_buffrw(struct brcmf_sdio_dev *sdiodev, uint fn,
         struct mmc_data mmc_dat;
         struct sg_table st;
         struct scatterlist *sgl;
-        struct mmc_host *host;
         int ret = 0;
 
         if (!pktlist->qlen)
@@ -351,27 +381,6 @@ static int brcmf_sdio_buffrw(struct brcmf_sdio_dev *sdiodev, uint fn,
         if (brcmf_pm_resume_error(sdiodev))
                 return -EIO;
 
-        /* Single skb use the standard mmc interface */
-        if (pktlist->qlen == 1) {
-                pkt_next = pktlist->next;
-                req_sz = pkt_next->len + 3;
-                req_sz &= (uint)~3;
-
-                if (write)
-                        return sdio_memcpy_toio(sdiodev->func[fn], addr,
-                                                ((u8 *)(pkt_next->data)),
-                                                req_sz);
-                else if (fn == 1)
-                        return sdio_memcpy_fromio(sdiodev->func[fn],
-                                                  ((u8 *)(pkt_next->data)),
-                                                  addr, req_sz);
-                else
-                        /* function 2 read is FIFO operation */
-                        return sdio_readsb(sdiodev->func[fn],
-                                           ((u8 *)(pkt_next->data)), addr,
-                                           req_sz);
-        }
-
         target_list = pktlist;
         /* for host with broken sg support, prepare a page aligned list */
         __skb_queue_head_init(&local_list);
@@ -398,38 +407,46 @@ static int brcmf_sdio_buffrw(struct brcmf_sdio_dev *sdiodev, uint fn,
                 target_list = &local_list;
         }
 
-        host = sdiodev->func[fn]->card->host;
         func_blk_sz = sdiodev->func[fn]->cur_blksize;
-        /* Blocks per command is limited by host count, host transfer
-         * size and the maximum for IO_RW_EXTENDED of 511 blocks.
-         */
-        max_blks = min_t(unsigned int, host->max_blk_count, 511u);
-        max_req_sz = min_t(unsigned int, host->max_req_size,
-                           max_blks * func_blk_sz);
-        max_seg_sz = min_t(unsigned short, host->max_segs, SG_MAX_SINGLE_ALLOC);
-        max_seg_sz = min_t(unsigned short, max_seg_sz, target_list->qlen);
+        max_req_sz = sdiodev->max_request_size;
+        max_seg_cnt = min_t(unsigned short, sdiodev->max_segment_count,
+                            target_list->qlen);
         seg_sz = target_list->qlen;
         pkt_offset = 0;
         pkt_next = target_list->next;
 
-        if (sg_alloc_table(&st, max_seg_sz, GFP_KERNEL)) {
+        if (sg_alloc_table(&st, max_seg_cnt, GFP_KERNEL)) {
                 ret = -ENOMEM;
                 goto exit;
         }
 
+        memset(&mmc_req, 0, sizeof(struct mmc_request));
+        memset(&mmc_cmd, 0, sizeof(struct mmc_command));
+        memset(&mmc_dat, 0, sizeof(struct mmc_data));
+
+        mmc_dat.sg = st.sgl;
+        mmc_dat.blksz = func_blk_sz;
+        mmc_dat.flags = write ? MMC_DATA_WRITE : MMC_DATA_READ;
+        mmc_cmd.opcode = SD_IO_RW_EXTENDED;
+        mmc_cmd.arg = write ? 1<<31 : 0;        /* write flag  */
+        mmc_cmd.arg |= (fn & 0x7) << 28;        /* SDIO func num */
+        mmc_cmd.arg |= 1<<27;                   /* block mode */
+        /* for function 1 the addr will be incremented */
+        mmc_cmd.arg |= (fn == 1) ? 1<<26 : 0;
+        mmc_cmd.flags = MMC_RSP_SPI_R5 | MMC_RSP_R5 | MMC_CMD_ADTC;
+        mmc_req.cmd = &mmc_cmd;
+        mmc_req.data = &mmc_dat;
+
         while (seg_sz) {
                 req_sz = 0;
                 sg_cnt = 0;
-                memset(&mmc_req, 0, sizeof(struct mmc_request));
-                memset(&mmc_cmd, 0, sizeof(struct mmc_command));
-                memset(&mmc_dat, 0, sizeof(struct mmc_data));
                 sgl = st.sgl;
                 /* prep sg table */
                 while (pkt_next != (struct sk_buff *)target_list) {
                         pkt_data = pkt_next->data + pkt_offset;
                         sg_data_sz = pkt_next->len - pkt_offset;
-                        if (sg_data_sz > host->max_seg_size)
-                                sg_data_sz = host->max_seg_size;
+                        if (sg_data_sz > sdiodev->max_segment_size)
+                                sg_data_sz = sdiodev->max_segment_size;
                         if (sg_data_sz > max_req_sz - req_sz)
                                 sg_data_sz = max_req_sz - req_sz;
 
@@ -444,7 +461,7 @@ static int brcmf_sdio_buffrw(struct brcmf_sdio_dev *sdiodev, uint fn,
                                 pkt_next = pkt_next->next;
                         }
 
-                        if (req_sz >= max_req_sz || sg_cnt >= max_seg_sz)
+                        if (req_sz >= max_req_sz || sg_cnt >= max_seg_cnt)
                                 break;
                 }
                 seg_sz -= sg_cnt;
@@ -455,27 +472,17 @@ static int brcmf_sdio_buffrw(struct brcmf_sdio_dev *sdiodev, uint fn,
                         ret = -ENOTBLK;
                         goto exit;
                 }
-                mmc_dat.sg = st.sgl;
+
                 mmc_dat.sg_len = sg_cnt;
-                mmc_dat.blksz = func_blk_sz;
                 mmc_dat.blocks = req_sz / func_blk_sz;
-                mmc_dat.flags = write ? MMC_DATA_WRITE : MMC_DATA_READ;
-                mmc_cmd.opcode = SD_IO_RW_EXTENDED;
-                mmc_cmd.arg = write ? 1<<31 : 0;        /* write flag  */
-                mmc_cmd.arg |= (fn & 0x7) << 28;        /* SDIO func num */
-                mmc_cmd.arg |= 1<<27;                   /* block mode */
-                /* incrementing addr for function 1 */
-                mmc_cmd.arg |= (fn == 1) ? 1<<26 : 0;
                 mmc_cmd.arg |= (addr & 0x1FFFF) << 9;   /* address */
                 mmc_cmd.arg |= mmc_dat.blocks & 0x1FF;  /* block count */
-                mmc_cmd.flags = MMC_RSP_SPI_R5 | MMC_RSP_R5 | MMC_CMD_ADTC;
-                mmc_req.cmd = &mmc_cmd;
-                mmc_req.data = &mmc_dat;
+                /* incrementing addr for function 1 */
                 if (fn == 1)
                         addr += req_sz;
 
                 mmc_set_data_timeout(&mmc_dat, sdiodev->func[fn]->card);
-                mmc_wait_for_req(host, &mmc_req);
+                mmc_wait_for_req(sdiodev->func[fn]->card->host, &mmc_req);
 
                 ret = mmc_cmd.error ? mmc_cmd.error : mmc_dat.error;
                 if (ret != 0) {
@@ -546,7 +553,6 @@ brcmf_sdcard_recv_pkt(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
 {
         uint width;
         int err = 0;
-        struct sk_buff_head pkt_list;
 
         brcmf_dbg(SDIO, "fun = %d, addr = 0x%x, size = %d\n",
                   fn, addr, pkt->len);
@@ -556,19 +562,17 @@ brcmf_sdcard_recv_pkt(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
         if (err)
                 goto done;
 
-        skb_queue_head_init(&pkt_list);
-        skb_queue_tail(&pkt_list, pkt);
-        err = brcmf_sdio_buffrw(sdiodev, fn, false, addr, &pkt_list);
-        skb_dequeue_tail(&pkt_list);
+        err = brcmf_sdio_buffrw(sdiodev, fn, false, addr, pkt);
 
 done:
         return err;
 }
 
 int brcmf_sdcard_recv_chain(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
-                            uint flags, struct sk_buff_head *pktq)
+                            uint flags, struct sk_buff_head *pktq, uint totlen)
 {
-        uint incr_fix;
+        struct sk_buff *glom_skb;
+        struct sk_buff *skb;
         uint width;
         int err = 0;
 
@@ -580,8 +584,22 @@ int brcmf_sdcard_recv_chain(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
         if (err)
                 goto done;
 
-        incr_fix = (flags & SDIO_REQ_FIXED) ? SDIOH_DATA_FIX : SDIOH_DATA_INC;
-        err = brcmf_sdio_buffrw(sdiodev, fn, false, addr, pktq);
+        if (pktq->qlen == 1)
+                err = brcmf_sdio_buffrw(sdiodev, fn, false, addr, pktq->next);
+        else if (!sdiodev->sg_support) {
+                glom_skb = brcmu_pkt_buf_get_skb(totlen);
+                if (!glom_skb)
+                        return -ENOMEM;
+                err = brcmf_sdio_buffrw(sdiodev, fn, false, addr, glom_skb);
+                if (err)
+                        goto done;
+
+                skb_queue_walk(pktq, skb) {
+                        memcpy(skb->data, glom_skb->data, skb->len);
+                        skb_pull(glom_skb, skb->len);
+                }
+        } else
+                err = brcmf_sdio_sglist_rw(sdiodev, fn, false, addr, pktq);
 
 done:
         return err;
@@ -592,7 +610,7 @@ brcmf_sdcard_send_buf(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
                       uint flags, u8 *buf, uint nbytes)
 {
         struct sk_buff *mypkt;
-        struct sk_buff_head pktq;
+        uint width;
         int err;
 
         mypkt = brcmu_pkt_buf_get_skb(nbytes);
@@ -603,10 +621,12 @@ brcmf_sdcard_send_buf(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
         }
 
         memcpy(mypkt->data, buf, nbytes);
-        __skb_queue_head_init(&pktq);
-        __skb_queue_tail(&pktq, mypkt);
-        err = brcmf_sdcard_send_pkt(sdiodev, addr, fn, flags, &pktq);
-        __skb_dequeue_tail(&pktq);
+
+        width = (flags & SDIO_REQ_4BYTE) ? 4 : 2;
+        err = brcmf_sdio_addrprep(sdiodev, width, &addr);
+
+        if (!err)
+                err = brcmf_sdio_buffrw(sdiodev, fn, true, addr, mypkt);
 
         brcmu_pkt_buf_free_skb(mypkt);
         return err;
@@ -617,16 +637,26 @@ int
 brcmf_sdcard_send_pkt(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
                       uint flags, struct sk_buff_head *pktq)
 {
+        struct sk_buff *skb;
         uint width;
-        int err = 0;
+        int err;
 
         brcmf_dbg(SDIO, "fun = %d, addr = 0x%x, size = %d\n",
                   fn, addr, pktq->qlen);
 
         width = (flags & SDIO_REQ_4BYTE) ? 4 : 2;
-        brcmf_sdio_addrprep(sdiodev, width, &addr);
+        err = brcmf_sdio_addrprep(sdiodev, width, &addr);
+        if (err)
+                return err;
 
-        err = brcmf_sdio_buffrw(sdiodev, fn, true, addr, pktq);
+        if (pktq->qlen == 1 || !sdiodev->sg_support)
+                skb_queue_walk(pktq, skb) {
+                        err = brcmf_sdio_buffrw(sdiodev, fn, true, addr, skb);
+                        if (err)
+                                break;
+                }
+        else
+                err = brcmf_sdio_sglist_rw(sdiodev, fn, true, addr, pktq);
 
         return err;
 }
@@ -639,7 +669,6 @@ brcmf_sdio_ramrw(struct brcmf_sdio_dev *sdiodev, bool write, u32 address,
         struct sk_buff *pkt;
         u32 sdaddr;
         uint dsize;
-        struct sk_buff_head pkt_list;
 
         dsize = min_t(uint, SBSDIO_SB_OFT_ADDR_LIMIT, size);
         pkt = dev_alloc_skb(dsize);
@@ -648,7 +677,6 @@ brcmf_sdio_ramrw(struct brcmf_sdio_dev *sdiodev, bool write, u32 address,
                 return -EIO;
         }
         pkt->priority = 0;
-        skb_queue_head_init(&pkt_list);
 
         /* Determine initial transfer parameters */
         sdaddr = address & SBSDIO_SB_OFT_ADDR_MASK;
@@ -676,10 +704,8 @@ brcmf_sdio_ramrw(struct brcmf_sdio_dev *sdiodev, bool write, u32 address,
                 skb_put(pkt, dsize);
                 if (write)
                         memcpy(pkt->data, data, dsize);
-                skb_queue_tail(&pkt_list, pkt);
                 bcmerror = brcmf_sdio_buffrw(sdiodev, SDIO_FUNC_1, write,
-                                             sdaddr, &pkt_list);
-                skb_dequeue_tail(&pkt_list);
+                                             sdaddr, pkt);
                 if (bcmerror) {
                         brcmf_err("membytes transfer failed\n");
                         break;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
index 2a23bf2b904d..905704e335d7 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh_sdmmc.c
@@ -21,6 +21,7 @@
 #include <linux/mmc/sdio_func.h>
 #include <linux/mmc/sdio_ids.h>
 #include <linux/mmc/card.h>
+#include <linux/mmc/host.h>
 #include <linux/suspend.h>
 #include <linux/errno.h>
 #include <linux/sched.h>        /* request_irq() */
@@ -315,6 +316,8 @@ static int brcmf_ops_sdio_probe(struct sdio_func *func,
         int err;
         struct brcmf_sdio_dev *sdiodev;
         struct brcmf_bus *bus_if;
+        struct mmc_host *host;
+        uint max_blocks;
 
         brcmf_dbg(SDIO, "Enter\n");
         brcmf_dbg(SDIO, "Class=%x\n", func->class);
@@ -361,6 +364,20 @@ static int brcmf_ops_sdio_probe(struct sdio_func *func,
                 brcmf_err("F2 error, probe failed %d...\n", err);
                 goto fail;
         }
+
+        /*
+         * determine host related variables after brcmf_sdio_probe()
+         * as func->cur_blksize is properly set and F2 init has been
+         * completed successfully.
+         */
+        host = func->card->host;
+        sdiodev->sg_support = host->max_segs > 1;
+        max_blocks = min_t(uint, host->max_blk_count, 511u);
+        sdiodev->max_request_size = min_t(uint, host->max_req_size,
+                                          max_blocks * func->cur_blksize);
+        sdiodev->max_segment_count = min_t(uint, host->max_segs,
+                                           SG_MAX_SINGLE_ALLOC);
+        sdiodev->max_segment_size = host->max_seg_size;
         brcmf_dbg(SDIO, "F2 init completed...\n");
         return 0;
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
index 67f05db4b9b8..b02953c4ade7 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c
@@ -1147,6 +1147,8 @@ static int brcmf_sdio_hdparse(struct brcmf_sdio *bus, u8 *header,
         u8 rx_seq, fc, tx_seq_max;
         u32 swheader;
 
+        trace_brcmf_sdpcm_hdr(false, header);
+
         /* hw header */
         len = get_unaligned_le16(header);
         checksum = get_unaligned_le16(header + sizeof(u16));
@@ -1269,6 +1271,7 @@ static void brcmf_sdio_hdpack(struct brcmf_sdio *bus, u8 *header,
                      SDPCM_DOFFSET_MASK;
         *(((__le32 *)header) + 1) = cpu_to_le32(sw_header);
         *(((__le32 *)header) + 2) = 0;
+        trace_brcmf_sdpcm_hdr(true, header);
 }
 
 static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
@@ -1389,7 +1392,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
                 sdio_claim_host(bus->sdiodev->func[1]);
                 errcode = brcmf_sdcard_recv_chain(bus->sdiodev,
                                 bus->sdiodev->sbwad,
-                                SDIO_FUNC_2, F2SYNC, &bus->glom);
+                                SDIO_FUNC_2, F2SYNC, &bus->glom, dlen);
                 sdio_release_host(bus->sdiodev->func[1]);
                 bus->sdcnt.f2rxdata++;
 
@@ -1877,6 +1880,56 @@ brcmf_sdbrcm_wait_event_wakeup(struct brcmf_sdio *bus)
 /* bit mask of data length chopped from the previous packet */
 #define ALIGN_SKB_CHOP_LEN_MASK 0x7fff
 
+static int brcmf_sdio_txpkt_prep_sg(struct brcmf_sdio_dev *sdiodev,
+                                    struct sk_buff_head *pktq,
+                                    struct sk_buff *pkt, uint chan)
+{
+        struct sk_buff *pkt_pad;
+        u16 tail_pad, tail_chop, sg_align;
+        unsigned int blksize;
+        u8 *dat_buf;
+        int ntail;
+
+        blksize = sdiodev->func[SDIO_FUNC_2]->cur_blksize;
+        sg_align = 4;
+        if (sdiodev->pdata && sdiodev->pdata->sd_sgentry_align > 4)
+                sg_align = sdiodev->pdata->sd_sgentry_align;
+        /* sg entry alignment should be a divisor of block size */
+        WARN_ON(blksize % sg_align);
+
+        /* Check tail padding */
+        pkt_pad = NULL;
+        tail_chop = pkt->len % sg_align;
+        tail_pad = sg_align - tail_chop;
+        tail_pad += blksize - (pkt->len + tail_pad) % blksize;
+        if (skb_tailroom(pkt) < tail_pad && pkt->len > blksize) {
+                pkt_pad = brcmu_pkt_buf_get_skb(tail_pad + tail_chop);
+                if (pkt_pad == NULL)
+                        return -ENOMEM;
+                memcpy(pkt_pad->data,
+                       pkt->data + pkt->len - tail_chop,
+                       tail_chop);
+                *(u32 *)(pkt_pad->cb) = ALIGN_SKB_FLAG + tail_chop;
+                skb_trim(pkt, pkt->len - tail_chop);
+                __skb_queue_after(pktq, pkt, pkt_pad);
+        } else {
+                ntail = pkt->data_len + tail_pad -
+                        (pkt->end - pkt->tail);
+                if (skb_cloned(pkt) || ntail > 0)
+                        if (pskb_expand_head(pkt, 0, ntail, GFP_ATOMIC))
+                                return -ENOMEM;
+                if (skb_linearize(pkt))
+                        return -ENOMEM;
+                dat_buf = (u8 *)(pkt->data);
+                __skb_put(pkt, tail_pad);
+        }
+
+        if (pkt_pad)
+                return pkt->len + tail_chop;
+        else
+                return pkt->len - tail_pad;
+}
+
 /**
  * brcmf_sdio_txpkt_prep - packet preparation for transmit
  * @bus: brcmf_sdio structure pointer
@@ -1893,24 +1946,16 @@ static int
 brcmf_sdio_txpkt_prep(struct brcmf_sdio *bus, struct sk_buff_head *pktq,
                       uint chan)
 {
-        u16 head_pad, tail_pad, tail_chop, head_align, sg_align;
-        int ntail;
-        struct sk_buff *pkt_next, *pkt_new;
+        u16 head_pad, head_align;
+        struct sk_buff *pkt_next;
         u8 *dat_buf;
-        unsigned blksize = bus->sdiodev->func[SDIO_FUNC_2]->cur_blksize;
+        int err;
         struct brcmf_sdio_hdrinfo hd_info = {0};
 
         /* SDIO ADMA requires at least 32 bit alignment */
         head_align = 4;
-        sg_align = 4;
-        if (bus->sdiodev->pdata) {
-                head_align = bus->sdiodev->pdata->sd_head_align > 4 ?
-                             bus->sdiodev->pdata->sd_head_align : 4;
-                sg_align = bus->sdiodev->pdata->sd_sgentry_align > 4 ?
-                           bus->sdiodev->pdata->sd_sgentry_align : 4;
-        }
-        /* sg entry alignment should be a divisor of block size */
-        WARN_ON(blksize % sg_align);
+        if (bus->sdiodev->pdata && bus->sdiodev->pdata->sd_head_align > 4)
+                head_align = bus->sdiodev->pdata->sd_head_align;
 
         pkt_next = pktq->next;
         dat_buf = (u8 *)(pkt_next->data);
@@ -1929,40 +1974,20 @@ brcmf_sdio_txpkt_prep(struct brcmf_sdio *bus, struct sk_buff_head *pktq,
                 memset(dat_buf, 0, head_pad + bus->tx_hdrlen);
         }
 
-        /* Check tail padding */
-        pkt_new = NULL;
-        tail_chop = pkt_next->len % sg_align;
-        tail_pad = sg_align - tail_chop;
-        tail_pad += blksize - (pkt_next->len + tail_pad) % blksize;
-        if (skb_tailroom(pkt_next) < tail_pad && pkt_next->len > blksize) {
-                pkt_new = brcmu_pkt_buf_get_skb(tail_pad + tail_chop);
-                if (pkt_new == NULL)
-                        return -ENOMEM;
-                memcpy(pkt_new->data,
-                       pkt_next->data + pkt_next->len - tail_chop,
-                       tail_chop);
-                *(u32 *)(pkt_new->cb) = ALIGN_SKB_FLAG + tail_chop;
-                skb_trim(pkt_next, pkt_next->len - tail_chop);
-                __skb_queue_after(pktq, pkt_next, pkt_new);
+        if (bus->sdiodev->sg_support && pktq->qlen > 1) {
+                err = brcmf_sdio_txpkt_prep_sg(bus->sdiodev, pktq,
+                                               pkt_next, chan);
+                if (err < 0)
+                        return err;
+                hd_info.len = (u16)err;
         } else {
-                ntail = pkt_next->data_len + tail_pad -
-                        (pkt_next->end - pkt_next->tail);
-                if (skb_cloned(pkt_next) || ntail > 0)
-                        if (pskb_expand_head(pkt_next, 0, ntail, GFP_ATOMIC))
-                                return -ENOMEM;
-                if (skb_linearize(pkt_next))
-                        return -ENOMEM;
-                dat_buf = (u8 *)(pkt_next->data);
-                __skb_put(pkt_next, tail_pad);
+                hd_info.len = pkt_next->len;
         }
 
-        /* Now prep the header */
-        if (pkt_new)
-                hd_info.len = pkt_next->len + tail_chop;
-        else
-                hd_info.len = pkt_next->len - tail_pad;
         hd_info.channel = chan;
         hd_info.dat_offset = head_pad + bus->tx_hdrlen;
+
+        /* Now fill the header */
         brcmf_sdio_hdpack(bus, dat_buf, &hd_info);
 
         if (BRCMF_BYTES_ON() &&
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h b/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
index c9b06b4e71f7..fc0d4f0129db 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/sdio_host.h
@@ -178,6 +178,10 @@ struct brcmf_sdio_dev {
         bool irq_en;                    /* irq enable flags */
         spinlock_t irq_en_lock;
         bool irq_wake;                  /* irq wake enable flags */
+        bool sg_support;
+        uint max_request_size;
+        ushort max_segment_count;
+        uint max_segment_size;
 };
 
 /* Register/deregister interrupt handler. */
@@ -216,7 +220,7 @@ int brcmf_sdcard_recv_pkt(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
 int brcmf_sdcard_recv_buf(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
                           uint flags, u8 *buf, uint nbytes);
 int brcmf_sdcard_recv_chain(struct brcmf_sdio_dev *sdiodev, u32 addr, uint fn,
-                            uint flags, struct sk_buff_head *pktq);
+                            uint flags, struct sk_buff_head *pktq, uint totlen);
 
 /* Flags bits */
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.h b/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.h
index bc2917112899..3c67529b9074 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.h
@@ -78,13 +78,15 @@ TRACE_EVENT(brcmf_hexdump,
         TP_ARGS(data, len),
         TP_STRUCT__entry(
                 __field(unsigned long, len)
+                __field(unsigned long, addr)
                 __dynamic_array(u8, hdata, len)
         ),
         TP_fast_assign(
                 __entry->len = len;
+                __entry->addr = (unsigned long)data;
                 memcpy(__get_dynamic_array(hdata), data, len);
         ),
-        TP_printk("hexdump [length=%lu]", __entry->len)
+        TP_printk("hexdump [addr=%lx, length=%lu]", __entry->addr, __entry->len)
 );
 
 TRACE_EVENT(brcmf_bdchdr,
@@ -108,6 +110,23 @@ TRACE_EVENT(brcmf_bdchdr,
         TP_printk("bdc: prio=%d siglen=%d", __entry->prio, __entry->siglen)
 );
 
+TRACE_EVENT(brcmf_sdpcm_hdr,
+        TP_PROTO(bool tx, void *data),
+        TP_ARGS(tx, data),
+        TP_STRUCT__entry(
+                __field(u8, tx)
+                __field(u16, len)
+                __array(u8, hdr, 12)
+        ),
+        TP_fast_assign(
+                memcpy(__entry->hdr, data, 12);
+                __entry->len = __entry->hdr[0] | (__entry->hdr[1] << 8);
+                __entry->tx = tx ? 1 : 0;
+        ),
+        TP_printk("sdpcm: %s len %u, seq %d", __entry->tx ? "TX" : "RX",
+                  __entry->len, __entry->hdr[4])
+);
+
 #ifdef CONFIG_BRCM_TRACING
 
 #undef TRACE_INCLUDE_PATH
diff --git a/drivers/net/wireless/iwlwifi/dvm/tx.c b/drivers/net/wireless/iwlwifi/dvm/tx.c
index da442b81370a..1fef5240e6ad 100644
--- a/drivers/net/wireless/iwlwifi/dvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/tx.c
@@ -433,27 +433,19 @@ int iwlagn_tx_skb(struct iwl_priv *priv,
         /* Copy MAC header from skb into command buffer */
         memcpy(tx_cmd->hdr, hdr, hdr_len);
 
+        txq_id = info->hw_queue;
+
         if (is_agg)
                 txq_id = priv->tid_data[sta_id][tid].agg.txq_id;
         else if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
                 /*
-                 * Send this frame after DTIM -- there's a special queue
-                 * reserved for this for contexts that support AP mode.
-                 */
-                txq_id = ctx->mcast_queue;
-
-                /*
                  * The microcode will clear the more data
                  * bit in the last frame it transmits.
                  */
                 hdr->frame_control |=
                         cpu_to_le16(IEEE80211_FCTL_MOREDATA);
-        } else if (info->flags & IEEE80211_TX_CTL_TX_OFFCHAN)
-                txq_id = IWL_AUX_QUEUE;
-        else
-                txq_id = ctx->ac_to_queue[skb_get_queue_mapping(skb)];
+        }
 
-        WARN_ON_ONCE(!is_agg && txq_id != info->hw_queue);
         WARN_ON_ONCE(is_agg &&
                      priv->queue_to_mac80211[txq_id] != info->hw_queue);
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-7000.c b/drivers/net/wireless/iwlwifi/iwl-7000.c
index 76e14c046d94..85879dbaa402 100644
--- a/drivers/net/wireless/iwlwifi/iwl-7000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-7000.c
@@ -83,6 +83,8 @@
 #define IWL7260_TX_POWER_VERSION        0xffff /* meaningless */
 #define IWL3160_NVM_VERSION             0x709
 #define IWL3160_TX_POWER_VERSION        0xffff /* meaningless */
+#define IWL7265_NVM_VERSION             0x0a1d
+#define IWL7265_TX_POWER_VERSION        0xffff /* meaningless */
 
 #define IWL7260_FW_PRE "iwlwifi-7260-"
 #define IWL7260_MODULE_FIRMWARE(api) IWL7260_FW_PRE __stringify(api) ".ucode"
@@ -90,6 +92,9 @@
 #define IWL3160_FW_PRE "iwlwifi-3160-"
 #define IWL3160_MODULE_FIRMWARE(api) IWL3160_FW_PRE __stringify(api) ".ucode"
 
+#define IWL7265_FW_PRE "iwlwifi-7265-"
+#define IWL7265_MODULE_FIRMWARE(api) IWL7265_FW_PRE __stringify(api) ".ucode"
+
 static const struct iwl_base_params iwl7000_base_params = {
         .eeprom_size = OTP_LOW_IMAGE_SIZE,
         .num_of_queues = IWLAGN_NUM_QUEUES,
@@ -182,5 +187,14 @@ const struct iwl_cfg iwl3160_n_cfg = {
         .nvm_calib_ver = IWL3160_TX_POWER_VERSION,
 };
 
+const struct iwl_cfg iwl7265_2ac_cfg = {
+        .name = "Intel(R) Dual Band Wireless AC 7265",
+        .fw_name_pre = IWL7265_FW_PRE,
+        IWL_DEVICE_7000,
+        .ht_params = &iwl7000_ht_params,
+        .nvm_ver = IWL7265_NVM_VERSION,
+        .nvm_calib_ver = IWL7265_TX_POWER_VERSION,
+};
+
 MODULE_FIRMWARE(IWL7260_MODULE_FIRMWARE(IWL7260_UCODE_API_OK));
 MODULE_FIRMWARE(IWL3160_MODULE_FIRMWARE(IWL3160_UCODE_API_OK));
diff --git a/drivers/net/wireless/iwlwifi/iwl-config.h b/drivers/net/wireless/iwlwifi/iwl-config.h
index b03c25e14903..18f232e8e812 100644
--- a/drivers/net/wireless/iwlwifi/iwl-config.h
+++ b/drivers/net/wireless/iwlwifi/iwl-config.h
@@ -293,6 +293,7 @@ extern const struct iwl_cfg iwl7260_n_cfg;
 extern const struct iwl_cfg iwl3160_2ac_cfg;
 extern const struct iwl_cfg iwl3160_2n_cfg;
 extern const struct iwl_cfg iwl3160_n_cfg;
+extern const struct iwl_cfg iwl7265_2ac_cfg;
 #endif /* CONFIG_IWLMVM */
 
 #endif /* __IWL_CONFIG_H__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/iwlwifi/iwl-csr.h
index a276af476e2d..54a4fdc631b7 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/iwlwifi/iwl-csr.h
@@ -394,6 +394,38 @@
 #define CSR_DRAM_INT_TBL_ENABLE         (1 << 31)
 #define CSR_DRAM_INIT_TBL_WRAP_CHECK    (1 << 27)
 
+/* SECURE boot registers */
+#define CSR_SECURE_BOOT_CONFIG_ADDR     (0x100)
+enum secure_boot_config_reg {
+        CSR_SECURE_BOOT_CONFIG_INSPECTOR_BURNED_IN_OTP  = 0x00000001,
+        CSR_SECURE_BOOT_CONFIG_INSPECTOR_NOT_REQ        = 0x00000002,
+};
+
+#define CSR_SECURE_BOOT_CPU1_STATUS_ADDR        (0x100)
+#define CSR_SECURE_BOOT_CPU2_STATUS_ADDR        (0x100)
+enum secure_boot_status_reg {
+        CSR_SECURE_BOOT_CPU_STATUS_VERF_STATUS          = 0x00000003,
+        CSR_SECURE_BOOT_CPU_STATUS_VERF_COMPLETED       = 0x00000002,
+        CSR_SECURE_BOOT_CPU_STATUS_VERF_SUCCESS         = 0x00000004,
+        CSR_SECURE_BOOT_CPU_STATUS_VERF_FAIL            = 0x00000008,
+        CSR_SECURE_BOOT_CPU_STATUS_SIGN_VERF_FAIL       = 0x00000010,
+};
+
+#define CSR_UCODE_LOAD_STATUS_ADDR      (0x100)
+enum secure_load_status_reg {
+        CSR_CPU_STATUS_LOADING_STARTED                  = 0x00000001,
+        CSR_CPU_STATUS_LOADING_COMPLETED                = 0x00000002,
+        CSR_CPU_STATUS_NUM_OF_LAST_COMPLETED            = 0x000000F8,
+        CSR_CPU_STATUS_NUM_OF_LAST_LOADED_BLOCK         = 0x0000FF00,
+};
+
+#define CSR_SECURE_INSPECTOR_CODE_ADDR  (0x100)
+#define CSR_SECURE_INSPECTOR_DATA_ADDR  (0x100)
+
+#define CSR_SECURE_TIME_OUT     (100)
+
+#define FH_TCSR_0_REG0 (0x1D00)
+
 /*
  * HBUS (Host-side Bus)
  *
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/iwlwifi/iwl-drv.c
index 99e1da3123c9..ff570027e9dd 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.c
@@ -483,6 +483,7 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
         const u8 *tlv_data;
         char buildstr[25];
         u32 build;
+        int num_of_cpus;
 
         if (len < sizeof(*ucode)) {
                 IWL_ERR(drv, "uCode has invalid length: %zd\n", len);
@@ -692,6 +693,42 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
                                 goto invalid_tlv_len;
                         drv->fw.phy_config = le32_to_cpup((__le32 *)tlv_data);
                         break;
+                 case IWL_UCODE_TLV_SECURE_SEC_RT:
+                        iwl_store_ucode_sec(pieces, tlv_data, IWL_UCODE_REGULAR,
+                                            tlv_len);
+                        drv->fw.mvm_fw = true;
+                        drv->fw.img[IWL_UCODE_REGULAR].is_secure = true;
+                        break;
+                case IWL_UCODE_TLV_SECURE_SEC_INIT:
+                        iwl_store_ucode_sec(pieces, tlv_data, IWL_UCODE_INIT,
+                                            tlv_len);
+                        drv->fw.mvm_fw = true;
+                        drv->fw.img[IWL_UCODE_INIT].is_secure = true;
+                        break;
+                case IWL_UCODE_TLV_SECURE_SEC_WOWLAN:
+                        iwl_store_ucode_sec(pieces, tlv_data, IWL_UCODE_WOWLAN,
+                                            tlv_len);
+                        drv->fw.mvm_fw = true;
+                        drv->fw.img[IWL_UCODE_WOWLAN].is_secure = true;
+                        break;
+                case IWL_UCODE_TLV_NUM_OF_CPU:
+                        if (tlv_len != sizeof(u32))
+                                goto invalid_tlv_len;
+                        num_of_cpus =
+                                le32_to_cpup((__le32 *)tlv_data);
+
+                        if (num_of_cpus == 2) {
+                                drv->fw.img[IWL_UCODE_REGULAR].is_dual_cpus =
+                                        true;
+                                drv->fw.img[IWL_UCODE_INIT].is_dual_cpus =
+                                        true;
+                                drv->fw.img[IWL_UCODE_WOWLAN].is_dual_cpus =
+                                        true;
+                        } else if ((num_of_cpus > 2) || (num_of_cpus < 1)) {
+                                IWL_ERR(drv, "Driver support upto 2 CPUs\n");
+                                return -EINVAL;
+                        }
+                        break;
                 default:
                         IWL_DEBUG_INFO(drv, "unknown TLV: %d\n", tlv_type);
                         break;
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-file.h b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
index 8b6c6fd95ed0..6c6c35c5228c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-file.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
@@ -121,6 +121,10 @@ enum iwl_ucode_tlv_type {
         IWL_UCODE_TLV_SEC_WOWLAN        = 21,
         IWL_UCODE_TLV_DEF_CALIB         = 22,
         IWL_UCODE_TLV_PHY_SKU           = 23,
+        IWL_UCODE_TLV_SECURE_SEC_RT     = 24,
+        IWL_UCODE_TLV_SECURE_SEC_INIT   = 25,
+        IWL_UCODE_TLV_SECURE_SEC_WOWLAN = 26,
+        IWL_UCODE_TLV_NUM_OF_CPU        = 27,
 };
 
 struct iwl_ucode_tlv {
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw.h b/drivers/net/wireless/iwlwifi/iwl-fw.h
index a1223680bc70..87b66a821ec8 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw.h
@@ -75,11 +75,23 @@
  * @IWL_UCODE_TLV_FLAGS_P2P: This uCode image supports P2P.
  * @IWL_UCODE_TLV_FLAGS_DW_BC_TABLE: The SCD byte count table is in DWORDS
  * @IWL_UCODE_TLV_FLAGS_UAPSD: This uCode image supports uAPSD
+ * @IWL_UCODE_TLV_FLAGS_SHORT_BL: 16 entries of black list instead of 64 in scan
+ *      offload profile config command.
  * @IWL_UCODE_TLV_FLAGS_RX_ENERGY_API: supports rx signal strength api
  * @IWL_UCODE_TLV_FLAGS_TIME_EVENT_API_V2: using the new time event API.
  * @IWL_UCODE_TLV_FLAGS_D3_6_IPV6_ADDRS: D3 image supports up to six
  *      (rather than two) IPv6 addresses
  * @IWL_UCODE_TLV_FLAGS_BF_UPDATED: new beacon filtering API
+ * @IWL_UCODE_TLV_FLAGS_NO_BASIC_SSID: not sending a probe with the SSID element
+ *      from the probe request template.
+ * @IWL_UCODE_TLV_FLAGS_D3_CONTINUITY_API: modified D3 API to allow keeping
+ *      connection when going back to D0
+ * @IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_SMALL: new NS offload (small version)
+ * @IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_LARGE: new NS offload (large version)
+ * @IWL_UCODE_TLV_FLAGS_SCHED_SCAN: this uCode image supports scheduled scan.
+ * @IWL_UCODE_TLV_FLAGS_STA_KEY_CMD: new ADD_STA and ADD_STA_KEY command API
+ * @IWL_UCODE_TLV_FLAGS_DEVICE_PS_CMD: support device wide power command
+ *      containing CAM (Continuous Active Mode) indication.
  */
 enum iwl_ucode_tlv_flag {
         IWL_UCODE_TLV_FLAGS_PAN                 = BIT(0),
@@ -87,11 +99,20 @@ enum iwl_ucode_tlv_flag {
         IWL_UCODE_TLV_FLAGS_MFP                 = BIT(2),
         IWL_UCODE_TLV_FLAGS_P2P                 = BIT(3),
         IWL_UCODE_TLV_FLAGS_DW_BC_TABLE         = BIT(4),
+        IWL_UCODE_TLV_FLAGS_NEWBT_COEX          = BIT(5),
         IWL_UCODE_TLV_FLAGS_UAPSD               = BIT(6),
+        IWL_UCODE_TLV_FLAGS_SHORT_BL            = BIT(7),
         IWL_UCODE_TLV_FLAGS_RX_ENERGY_API       = BIT(8),
         IWL_UCODE_TLV_FLAGS_TIME_EVENT_API_V2   = BIT(9),
         IWL_UCODE_TLV_FLAGS_D3_6_IPV6_ADDRS     = BIT(10),
         IWL_UCODE_TLV_FLAGS_BF_UPDATED          = BIT(11),
+        IWL_UCODE_TLV_FLAGS_NO_BASIC_SSID       = BIT(12),
+        IWL_UCODE_TLV_FLAGS_D3_CONTINUITY_API   = BIT(14),
+        IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_SMALL    = BIT(15),
+        IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_LARGE    = BIT(16),
+        IWL_UCODE_TLV_FLAGS_SCHED_SCAN          = BIT(17),
+        IWL_UCODE_TLV_FLAGS_STA_KEY_CMD         = BIT(19),
+        IWL_UCODE_TLV_FLAGS_DEVICE_PS_CMD       = BIT(20),
 };
 
 /* The default calibrate table size if not specified by firmware file */
@@ -133,7 +154,8 @@ enum iwl_ucode_sec {
  * For 16.0 uCode and above, there is no differentiation between sections,
  * just an offset to the HW address.
  */
-#define IWL_UCODE_SECTION_MAX 4
+#define IWL_UCODE_SECTION_MAX 6
+#define IWL_UCODE_FIRST_SECTION_OF_SECOND_CPU   (IWL_UCODE_SECTION_MAX/2)
 
 struct iwl_ucode_capabilities {
         u32 max_probe_length;
@@ -150,6 +172,8 @@ struct fw_desc {
 
 struct fw_img {
         struct fw_desc sec[IWL_UCODE_SECTION_MAX];
+        bool is_secure;
+        bool is_dual_cpus;
 };
 
 /* uCode version contains 4 values: Major/Minor/API/Serial */
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
index ff8cc75c189d..a70c7b9d9bad 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -97,6 +97,8 @@
 
 #define APMG_PCIDEV_STT_VAL_L1_ACT_DIS          (0x00000800)
 
+#define APMG_RTC_INT_STT_RFKILL         (0x10000000)
+
 /* Device system time */
 #define DEVICE_SYSTEM_TIME_REG 0xA0206C
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.h b/drivers/net/wireless/iwlwifi/iwl-trans.h
index 80b47508647c..c6bac7c90b00 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.h
@@ -601,7 +601,7 @@ static inline int iwl_trans_send_cmd(struct iwl_trans *trans,
 {
         int ret;
 
-        if (trans->state != IWL_TRANS_FW_ALIVE) {
+        if (unlikely(trans->state != IWL_TRANS_FW_ALIVE)) {
                 IWL_ERR(trans, "%s bad state = %d", __func__, trans->state);
                 return -EIO;
         }
@@ -640,8 +640,8 @@ static inline void iwl_trans_free_tx_cmd(struct iwl_trans *trans,
 static inline int iwl_trans_tx(struct iwl_trans *trans, struct sk_buff *skb,
                                struct iwl_device_cmd *dev_cmd, int queue)
 {
-        WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
-                  "%s bad state = %d", __func__, trans->state);
+        if (unlikely(trans->state != IWL_TRANS_FW_ALIVE))
+                IWL_ERR(trans, "%s bad state = %d", __func__, trans->state);
 
         return trans->ops->tx(trans, skb, dev_cmd, queue);
 }
@@ -649,16 +649,16 @@ static inline int iwl_trans_tx(struct iwl_trans *trans, struct sk_buff *skb,
 static inline void iwl_trans_reclaim(struct iwl_trans *trans, int queue,
                                      int ssn, struct sk_buff_head *skbs)
 {
-        WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
-                  "%s bad state = %d", __func__, trans->state);
+        if (unlikely(trans->state != IWL_TRANS_FW_ALIVE))
+                IWL_ERR(trans, "%s bad state = %d", __func__, trans->state);
 
         trans->ops->reclaim(trans, queue, ssn, skbs);
 }
 
 static inline void iwl_trans_txq_disable(struct iwl_trans *trans, int queue)
 {
-        WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
-                  "%s bad state = %d", __func__, trans->state);
+        if (unlikely(trans->state != IWL_TRANS_FW_ALIVE))
+                IWL_ERR(trans, "%s bad state = %d", __func__, trans->state);
 
         trans->ops->txq_disable(trans, queue);
 }
@@ -669,8 +669,8 @@ static inline void iwl_trans_txq_enable(struct iwl_trans *trans, int queue,
 {
         might_sleep();
 
-        WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
-                  "%s bad state = %d", __func__, trans->state);
+        if (unlikely((trans->state != IWL_TRANS_FW_ALIVE)))
+                IWL_ERR(trans, "%s bad state = %d", __func__, trans->state);
 
         trans->ops->txq_enable(trans, queue, fifo, sta_id, tid,
                                  frame_limit, ssn);
@@ -685,8 +685,8 @@ static inline void iwl_trans_ac_txq_enable(struct iwl_trans *trans, int queue,
 
 static inline int iwl_trans_wait_tx_queue_empty(struct iwl_trans *trans)
 {
-        WARN_ONCE(trans->state != IWL_TRANS_FW_ALIVE,
-                  "%s bad state = %d", __func__, trans->state);
+        if (unlikely(trans->state != IWL_TRANS_FW_ALIVE))
+                IWL_ERR(trans, "%s bad state = %d", __func__, trans->state);
 
         return trans->ops->wait_tx_queue_empty(trans);
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/bt-coex.c b/drivers/net/wireless/iwlwifi/mvm/bt-coex.c
index 0fad98b85f60..5b630f12bbff 100644
--- a/drivers/net/wireless/iwlwifi/mvm/bt-coex.c
+++ b/drivers/net/wireless/iwlwifi/mvm/bt-coex.c
@@ -98,126 +98,258 @@ static const u8 iwl_bt_prio_tbl[BT_COEX_PRIO_TBL_EVT_MAX] = {
 
 #undef EVENT_PRIO_ANT
 
-/* BT Antenna Coupling Threshold (dB) */
-#define IWL_BT_ANTENNA_COUPLING_THRESHOLD       (35)
-#define IWL_BT_LOAD_FORCE_SISO_THRESHOLD        (3)
-
 #define BT_ENABLE_REDUCED_TXPOWER_THRESHOLD     (-62)
 #define BT_DISABLE_REDUCED_TXPOWER_THRESHOLD    (-65)
-#define BT_REDUCED_TX_POWER_BIT                 BIT(7)
-
-static inline bool is_loose_coex(void)
-{
-        return iwlwifi_mod_params.ant_coupling >
-                IWL_BT_ANTENNA_COUPLING_THRESHOLD;
-}
+#define BT_ANTENNA_COUPLING_THRESHOLD           (30)
 
 int iwl_send_bt_prio_tbl(struct iwl_mvm *mvm)
 {
+        if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_NEWBT_COEX))
+                return 0;
+
         return iwl_mvm_send_cmd_pdu(mvm, BT_COEX_PRIO_TABLE, CMD_SYNC,
                                     sizeof(struct iwl_bt_coex_prio_tbl_cmd),
                                     &iwl_bt_prio_tbl);
 }
 
-static int iwl_send_bt_env(struct iwl_mvm *mvm, u8 action, u8 type)
-{
-        struct iwl_bt_coex_prot_env_cmd env_cmd;
-        int ret;
-
-        env_cmd.action = action;
-        env_cmd.type = type;
-        ret = iwl_mvm_send_cmd_pdu(mvm, BT_COEX_PROT_ENV, CMD_SYNC,
-                                   sizeof(env_cmd), &env_cmd);
-        if (ret)
-                IWL_ERR(mvm, "failed to send BT env command\n");
-        return ret;
-}
-
-enum iwl_bt_kill_msk {
-        BT_KILL_MSK_DEFAULT,
-        BT_KILL_MSK_SCO_HID_A2DP,
-        BT_KILL_MSK_REDUCED_TXPOW,
-        BT_KILL_MSK_MAX,
-};
-
-static const u32 iwl_bt_ack_kill_msk[BT_KILL_MSK_MAX] = {
+const u32 iwl_bt_ack_kill_msk[BT_KILL_MSK_MAX] = {
         [BT_KILL_MSK_DEFAULT] = 0xffff0000,
         [BT_KILL_MSK_SCO_HID_A2DP] = 0xffffffff,
         [BT_KILL_MSK_REDUCED_TXPOW] = 0,
 };
 
-static const u32 iwl_bt_cts_kill_msk[BT_KILL_MSK_MAX] = {
+const u32 iwl_bt_cts_kill_msk[BT_KILL_MSK_MAX] = {
         [BT_KILL_MSK_DEFAULT] = 0xffff0000,
         [BT_KILL_MSK_SCO_HID_A2DP] = 0xffffffff,
         [BT_KILL_MSK_REDUCED_TXPOW] = 0,
 };
 
-#define IWL_BT_DEFAULT_BOOST (0xf0f0f0f0)
-
-/* Tight Coex */
-static const __le32 iwl_tight_lookup[BT_COEX_LUT_SIZE] = {
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xaeaaaaaa),
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xcc00ff28),
-        cpu_to_le32(0x0000aaaa),
-        cpu_to_le32(0xcc00aaaa),
-        cpu_to_le32(0x0000aaaa),
-        cpu_to_le32(0xc0004000),
-        cpu_to_le32(0x00000000),
-        cpu_to_le32(0xf0005000),
-        cpu_to_le32(0xf0005000),
+static const __le32 iwl_bt_prio_boost[BT_COEX_BOOST_SIZE] = {
+        cpu_to_le32(0xf0f0f0f0),
+        cpu_to_le32(0xc0c0c0c0),
+        cpu_to_le32(0xfcfcfcfc),
+        cpu_to_le32(0xff00ff00),
 };
 
-/* Loose Coex */
-static const __le32 iwl_loose_lookup[BT_COEX_LUT_SIZE] = {
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xcc00ff28),
-        cpu_to_le32(0x0000aaaa),
-        cpu_to_le32(0xcc00aaaa),
-        cpu_to_le32(0x0000aaaa),
-        cpu_to_le32(0x00000000),
-        cpu_to_le32(0x00000000),
-        cpu_to_le32(0xf0005000),
-        cpu_to_le32(0xf0005000),
+static const __le32 iwl_single_shared_ant[BT_COEX_MAX_LUT][BT_COEX_LUT_SIZE] = {
+        {
+                cpu_to_le32(0x40000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0x44000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0x40000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0x44000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0xc0004000),
+                cpu_to_le32(0xf0005000),
+                cpu_to_le32(0xc0004000),
+                cpu_to_le32(0xf0005000),
+        },
+        {
+                cpu_to_le32(0x40000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0x44000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0x40000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0x44000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0xc0004000),
+                cpu_to_le32(0xf0005000),
+                cpu_to_le32(0xc0004000),
+                cpu_to_le32(0xf0005000),
+        },
+        {
+                cpu_to_le32(0x40000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0x44000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0x40000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0x44000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0xc0004000),
+                cpu_to_le32(0xf0005000),
+                cpu_to_le32(0xc0004000),
+                cpu_to_le32(0xf0005000),
+        },
 };
 
-/* Full concurrency */
-static const __le32 iwl_concurrent_lookup[BT_COEX_LUT_SIZE] = {
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0xaaaaaaaa),
-        cpu_to_le32(0x00000000),
-        cpu_to_le32(0x00000000),
-        cpu_to_le32(0x00000000),
-        cpu_to_le32(0x00000000),
+static const __le32 iwl_combined_lookup[BT_COEX_MAX_LUT][BT_COEX_LUT_SIZE] = {
+        {
+                /* Tight */
+                cpu_to_le32(0xaaaaaaaa),
+                cpu_to_le32(0xaaaaaaaa),
+                cpu_to_le32(0xaeaaaaaa),
+                cpu_to_le32(0xaaaaaaaa),
+                cpu_to_le32(0xcc00ff28),
+                cpu_to_le32(0x0000aaaa),
+                cpu_to_le32(0xcc00aaaa),
+                cpu_to_le32(0x0000aaaa),
+                cpu_to_le32(0xc0004000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0xf0005000),
+                cpu_to_le32(0xf0005000),
+        },
+        {
+                /* Loose */
+                cpu_to_le32(0xaaaaaaaa),
+                cpu_to_le32(0xaaaaaaaa),
+                cpu_to_le32(0xaaaaaaaa),
+                cpu_to_le32(0xaaaaaaaa),
+                cpu_to_le32(0xcc00ff28),
+                cpu_to_le32(0x0000aaaa),
+                cpu_to_le32(0xcc00aaaa),
+                cpu_to_le32(0x0000aaaa),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0x00000000),
+                cpu_to_le32(0xf0005000),
+                cpu_to_le32(0xf0005000),
+        },
+        {
+                /* Tx Tx disabled */
+                cpu_to_le32(0xaaaaaaaa),
+                cpu_to_le32(0xaaaaaaaa),
+                cpu_to_le32(0xaaaaaaaa),
+                cpu_to_le32(0xaaaaaaaa),
+                cpu_to_le32(0xcc00ff28),
+                cpu_to_le32(0x0000aaaa),
+                cpu_to_le32(0xcc00aaaa),
+                cpu_to_le32(0x0000aaaa),
+                cpu_to_le32(0xC0004000),
+                cpu_to_le32(0xC0004000),
+                cpu_to_le32(0xF0005000),
+                cpu_to_le32(0xF0005000),
+        },
 };
 
-/* single shared antenna */
-static const __le32 iwl_single_shared_ant_lookup[BT_COEX_LUT_SIZE] = {
-        cpu_to_le32(0x40000000),
-        cpu_to_le32(0x00000000),
-        cpu_to_le32(0x44000000),
-        cpu_to_le32(0x00000000),
-        cpu_to_le32(0x40000000),
-        cpu_to_le32(0x00000000),
-        cpu_to_le32(0x44000000),
-        cpu_to_le32(0x00000000),
-        cpu_to_le32(0xC0004000),
-        cpu_to_le32(0xF0005000),
-        cpu_to_le32(0xC0004000),
-        cpu_to_le32(0xF0005000),
+/* 20MHz / 40MHz below / 40Mhz above*/
+static const __le64 iwl_ci_mask[][3] = {
+        /* dummy entry for channel 0 */
+        {cpu_to_le64(0), cpu_to_le64(0), cpu_to_le64(0)},
+        {
+                cpu_to_le64(0x0000001FFFULL),
+                cpu_to_le64(0x0ULL),
+                cpu_to_le64(0x00007FFFFFULL),
+        },
+        {
+                cpu_to_le64(0x000000FFFFULL),
+                cpu_to_le64(0x0ULL),
+                cpu_to_le64(0x0003FFFFFFULL),
+        },
+        {
+                cpu_to_le64(0x000003FFFCULL),
+                cpu_to_le64(0x0ULL),
+                cpu_to_le64(0x000FFFFFFCULL),
+        },
+        {
+                cpu_to_le64(0x00001FFFE0ULL),
+                cpu_to_le64(0x0ULL),
+                cpu_to_le64(0x007FFFFFE0ULL),
+        },
+        {
+                cpu_to_le64(0x00007FFF80ULL),
+                cpu_to_le64(0x00007FFFFFULL),
+                cpu_to_le64(0x01FFFFFF80ULL),
+        },
+        {
+                cpu_to_le64(0x0003FFFC00ULL),
+                cpu_to_le64(0x0003FFFFFFULL),
+                cpu_to_le64(0x0FFFFFFC00ULL),
+        },
+        {
+                cpu_to_le64(0x000FFFF000ULL),
+                cpu_to_le64(0x000FFFFFFCULL),
+                cpu_to_le64(0x3FFFFFF000ULL),
+        },
+        {
+                cpu_to_le64(0x007FFF8000ULL),
+                cpu_to_le64(0x007FFFFFE0ULL),
+                cpu_to_le64(0xFFFFFF8000ULL),
+        },
+        {
+                cpu_to_le64(0x01FFFE0000ULL),
+                cpu_to_le64(0x01FFFFFF80ULL),
+                cpu_to_le64(0xFFFFFE0000ULL),
+        },
+        {
+                cpu_to_le64(0x0FFFF00000ULL),
+                cpu_to_le64(0x0FFFFFFC00ULL),
+                cpu_to_le64(0x0ULL),
+        },
+        {
+                cpu_to_le64(0x3FFFC00000ULL),
+                cpu_to_le64(0x3FFFFFF000ULL),
+                cpu_to_le64(0x0)
+        },
+        {
+                cpu_to_le64(0xFFFE000000ULL),
+                cpu_to_le64(0xFFFFFF8000ULL),
+                cpu_to_le64(0x0)
+        },
+        {
+                cpu_to_le64(0xFFF8000000ULL),
+                cpu_to_le64(0xFFFFFE0000ULL),
+                cpu_to_le64(0x0)
+        },
+        {
+                cpu_to_le64(0xFE00000000ULL),
+                cpu_to_le64(0x0ULL),
+                cpu_to_le64(0x0)
+        },
 };
 
+static const __le32 iwl_bt_mprio_lut[BT_COEX_MULTI_PRIO_LUT_SIZE] = {
+        cpu_to_le32(0x22002200),
+        cpu_to_le32(0x33113311),
+};
+
+static enum iwl_bt_coex_lut_type
+iwl_get_coex_type(struct iwl_mvm *mvm, const struct ieee80211_vif *vif)
+{
+        struct ieee80211_chanctx_conf *chanctx_conf;
+        enum iwl_bt_coex_lut_type ret;
+        u16 phy_ctx_id;
+
+        /*
+         * Checking that we hold mvm->mutex is a good idea, but the rate
+         * control can't acquire the mutex since it runs in Tx path.
+         * So this is racy in that case, but in the worst case, the AMPDU
+         * size limit will be wrong for a short time which is not a big
+         * issue.
+         */
+
+        rcu_read_lock();
+
+        chanctx_conf = rcu_dereference(vif->chanctx_conf);
+
+        if (!chanctx_conf ||
+             chanctx_conf->def.chan->band != IEEE80211_BAND_2GHZ) {
+                rcu_read_unlock();
+                return BT_COEX_LOOSE_LUT;
+        }
+
+        ret = BT_COEX_TX_DIS_LUT;
+
+        if (mvm->cfg->bt_shared_single_ant) {
+                rcu_read_unlock();
+                return ret;
+        }
+
+        phy_ctx_id = *((u16 *)chanctx_conf->drv_priv);
+
+        if (mvm->last_bt_ci_cmd.primary_ch_phy_id == phy_ctx_id)
+                ret = le32_to_cpu(mvm->last_bt_notif.primary_ch_lut);
+        else if (mvm->last_bt_ci_cmd.secondary_ch_phy_id == phy_ctx_id)
+                ret = le32_to_cpu(mvm->last_bt_notif.secondary_ch_lut);
+        /* else - default = TX TX disallowed */
+
+        rcu_read_unlock();
+
+        return ret;
+}
+
 int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
 {
         struct iwl_bt_coex_cmd *bt_cmd;
@@ -228,17 +360,10 @@ int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
                 .flags = CMD_SYNC,
         };
         int ret;
+        u32 flags;
 
-        /* go to CALIB state in internal BT-Coex state machine */
-        ret = iwl_send_bt_env(mvm, BT_COEX_ENV_OPEN,
-                              BT_COEX_PRIO_TBL_EVT_INIT_CALIB2);
-        if (ret)
-                return ret;
-
-        ret  = iwl_send_bt_env(mvm, BT_COEX_ENV_CLOSE,
-                               BT_COEX_PRIO_TBL_EVT_INIT_CALIB2);
-        if (ret)
-                return ret;
+        if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_NEWBT_COEX))
+                return 0;
 
         bt_cmd = kzalloc(sizeof(*bt_cmd), GFP_KERNEL);
         if (!bt_cmd)
@@ -246,40 +371,52 @@ int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
         cmd.data[0] = bt_cmd;
 
         bt_cmd->max_kill = 5;
-        bt_cmd->bt3_time_t7_value = 1;
-        bt_cmd->bt3_prio_sample_time = 2;
-        bt_cmd->bt3_timer_t2_value = 0xc;
+        bt_cmd->bt4_antenna_isolation_thr = BT_ANTENNA_COUPLING_THRESHOLD,
+        bt_cmd->bt4_antenna_isolation = iwlwifi_mod_params.ant_coupling,
+        bt_cmd->bt4_tx_tx_delta_freq_thr = 15,
+        bt_cmd->bt4_tx_rx_max_freq0 = 15,
 
-        bt_cmd->flags = iwlwifi_mod_params.bt_coex_active ?
+        flags = iwlwifi_mod_params.bt_coex_active ?
                         BT_COEX_NW : BT_COEX_DISABLE;
-        bt_cmd->flags |= BT_CH_PRIMARY_EN | BT_SYNC_2_BT_DISABLE;
+        flags |= BT_CH_PRIMARY_EN | BT_CH_SECONDARY_EN | BT_SYNC_2_BT_DISABLE;
+        bt_cmd->flags = cpu_to_le32(flags);
 
-        bt_cmd->valid_bit_msk = cpu_to_le16(BT_VALID_ENABLE |
+        bt_cmd->valid_bit_msk = cpu_to_le32(BT_VALID_ENABLE |
                                             BT_VALID_BT_PRIO_BOOST |
                                             BT_VALID_MAX_KILL |
                                             BT_VALID_3W_TMRS |
                                             BT_VALID_KILL_ACK |
                                             BT_VALID_KILL_CTS |
                                             BT_VALID_REDUCED_TX_POWER |
-                                            BT_VALID_LUT);
+                                            BT_VALID_LUT |
+                                            BT_VALID_WIFI_RX_SW_PRIO_BOOST |
+                                            BT_VALID_WIFI_TX_SW_PRIO_BOOST |
+                                            BT_VALID_MULTI_PRIO_LUT |
+                                            BT_VALID_CORUN_LUT_20 |
+                                            BT_VALID_CORUN_LUT_40 |
+                                            BT_VALID_ANT_ISOLATION |
+                                            BT_VALID_ANT_ISOLATION_THRS |
+                                            BT_VALID_TXTX_DELTA_FREQ_THRS |
+                                            BT_VALID_TXRX_MAX_FREQ_0);
 
         if (mvm->cfg->bt_shared_single_ant)
-                memcpy(&bt_cmd->decision_lut, iwl_single_shared_ant_lookup,
-                       sizeof(iwl_single_shared_ant_lookup));
-        else if (is_loose_coex())
-                memcpy(&bt_cmd->decision_lut, iwl_loose_lookup,
-                       sizeof(iwl_tight_lookup));
+                memcpy(&bt_cmd->decision_lut, iwl_single_shared_ant,
+                       sizeof(iwl_single_shared_ant));
         else
-                memcpy(&bt_cmd->decision_lut, iwl_tight_lookup,
-                       sizeof(iwl_tight_lookup));
+                memcpy(&bt_cmd->decision_lut, iwl_combined_lookup,
+                       sizeof(iwl_combined_lookup));
 
-        bt_cmd->bt_prio_boost = cpu_to_le32(IWL_BT_DEFAULT_BOOST);
+        memcpy(&bt_cmd->bt_prio_boost, iwl_bt_prio_boost,
+               sizeof(iwl_bt_prio_boost));
+        memcpy(&bt_cmd->bt4_multiprio_lut, iwl_bt_mprio_lut,
+               sizeof(iwl_bt_mprio_lut));
         bt_cmd->kill_ack_msk =
                 cpu_to_le32(iwl_bt_ack_kill_msk[BT_KILL_MSK_DEFAULT]);
         bt_cmd->kill_cts_msk =
                 cpu_to_le32(iwl_bt_cts_kill_msk[BT_KILL_MSK_DEFAULT]);
 
         memset(&mvm->last_bt_notif, 0, sizeof(mvm->last_bt_notif));
+        memset(&mvm->last_bt_ci_cmd, 0, sizeof(mvm->last_bt_ci_cmd));
 
         ret = iwl_mvm_send_cmd(mvm, &cmd);
 
@@ -334,13 +471,17 @@ static int iwl_mvm_bt_udpate_ctrl_kill_msk(struct iwl_mvm *mvm,
         if (!bt_cmd)
                 return -ENOMEM;
         cmd.data[0] = bt_cmd;
+        bt_cmd->flags = cpu_to_le32(BT_COEX_NW);
 
         bt_cmd->kill_ack_msk = cpu_to_le32(iwl_bt_ack_kill_msk[bt_kill_msk]);
         bt_cmd->kill_cts_msk = cpu_to_le32(iwl_bt_cts_kill_msk[bt_kill_msk]);
-        bt_cmd->valid_bit_msk =
-                cpu_to_le16(BT_VALID_KILL_ACK | BT_VALID_KILL_CTS);
+        bt_cmd->valid_bit_msk |= cpu_to_le32(BT_VALID_ENABLE |
+                                             BT_VALID_KILL_ACK |
+                                             BT_VALID_KILL_CTS);
 
-        IWL_DEBUG_COEX(mvm, "bt_kill_msk = %d\n", bt_kill_msk);
+        IWL_DEBUG_COEX(mvm, "ACK Kill msk = 0x%08x, CTS Kill msk = 0x%08x\n",
+                       iwl_bt_ack_kill_msk[bt_kill_msk],
+                       iwl_bt_cts_kill_msk[bt_kill_msk]);
 
         ret = iwl_mvm_send_cmd(mvm, &cmd);
 
@@ -380,8 +521,10 @@ static int iwl_mvm_bt_coex_reduced_txp(struct iwl_mvm *mvm, u8 sta_id,
         if (!bt_cmd)
                 return -ENOMEM;
         cmd.data[0] = bt_cmd;
+        bt_cmd->flags = cpu_to_le32(BT_COEX_NW);
 
-        bt_cmd->valid_bit_msk = cpu_to_le16(BT_VALID_REDUCED_TX_POWER),
+        bt_cmd->valid_bit_msk =
+                cpu_to_le32(BT_VALID_ENABLE | BT_VALID_REDUCED_TX_POWER);
         bt_cmd->bt_reduced_tx_power = sta_id;
 
         if (enable)
@@ -403,8 +546,25 @@ struct iwl_bt_iterator_data {
         struct iwl_mvm *mvm;
         u32 num_bss_ifaces;
         bool reduced_tx_power;
+        struct ieee80211_chanctx_conf *primary;
+        struct ieee80211_chanctx_conf *secondary;
 };
 
+static inline
+void iwl_mvm_bt_coex_enable_rssi_event(struct iwl_mvm *mvm,
+                                       struct ieee80211_vif *vif,
+                                       bool enable, int rssi)
+{
+        struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+
+        mvmvif->bf_data.last_bt_coex_event = rssi;
+        mvmvif->bf_data.bt_coex_max_thold =
+                enable ? BT_ENABLE_REDUCED_TXPOWER_THRESHOLD : 0;
+        mvmvif->bf_data.bt_coex_min_thold =
+                enable ? BT_DISABLE_REDUCED_TXPOWER_THRESHOLD : 0;
+}
+
+/* must be called under rcu_read_lock */
 static void iwl_mvm_bt_notif_iterator(void *_data, u8 *mac,
                                       struct ieee80211_vif *vif)
 {
@@ -413,65 +573,94 @@ static void iwl_mvm_bt_notif_iterator(void *_data, u8 *mac,
         struct iwl_mvm *mvm = data->mvm;
         struct ieee80211_chanctx_conf *chanctx_conf;
         enum ieee80211_smps_mode smps_mode;
-        enum ieee80211_band band;
         int ave_rssi;
 
         lockdep_assert_held(&mvm->mutex);
-        if (vif->type != NL80211_IFTYPE_STATION)
-                return;
 
-        rcu_read_lock();
-        chanctx_conf = rcu_dereference(vif->chanctx_conf);
-        if (chanctx_conf && chanctx_conf->def.chan)
-                band = chanctx_conf->def.chan->band;
-        else
-                band = -1;
-        rcu_read_unlock();
+        if (vif->type != NL80211_IFTYPE_STATION &&
+            vif->type != NL80211_IFTYPE_AP)
+                return;
 
         smps_mode = IEEE80211_SMPS_AUTOMATIC;
 
-        /* non associated BSSes aren't to be considered */
-        if (!vif->bss_conf.assoc)
+        chanctx_conf = rcu_dereference(vif->chanctx_conf);
+
+        /* If channel context is invalid or not on 2.4GHz .. */
+        if ((!chanctx_conf ||
+             chanctx_conf->def.chan->band != IEEE80211_BAND_2GHZ)) {
+                /* ... and it is an associated STATION, relax constraints */
+                if (vif->type == NL80211_IFTYPE_STATION && vif->bss_conf.assoc)
+                        iwl_mvm_update_smps(mvm, vif, IWL_MVM_SMPS_REQ_BT_COEX,
+                                            smps_mode);
+                iwl_mvm_bt_coex_enable_rssi_event(mvm, vif, false, 0);
                 return;
+        }
+
+        /* SoftAP / GO will always be primary */
+        if (vif->type == NL80211_IFTYPE_AP) {
+                if (!mvmvif->ap_ibss_active)
+                        return;
+
+                /* the Ack / Cts kill mask must be default if AP / GO */
+                data->reduced_tx_power = false;
 
-        if (band != IEEE80211_BAND_2GHZ) {
-                iwl_mvm_update_smps(mvm, vif, IWL_MVM_SMPS_REQ_BT_COEX,
-                                    smps_mode);
+                if (chanctx_conf == data->primary)
+                        return;
+
+                /* downgrade the current primary no matter what its type is */
+                data->secondary = data->primary;
+                data->primary = chanctx_conf;
                 return;
         }
 
-        if (data->notif->bt_status)
-                smps_mode = IEEE80211_SMPS_DYNAMIC;
+        data->num_bss_ifaces++;
+
+        /* we are now a STA / P2P Client, and take associated ones only */
+        if (!vif->bss_conf.assoc)
+                return;
+
+        /* STA / P2P Client, try to be primary if first vif */
+        if (!data->primary || data->primary == chanctx_conf)
+                data->primary = chanctx_conf;
+        else if (!data->secondary)
+                /* if secondary is not NULL, it might be a GO */
+                data->secondary = chanctx_conf;
 
-        if (data->notif->bt_traffic_load >= IWL_BT_LOAD_FORCE_SISO_THRESHOLD)
+        if (le32_to_cpu(data->notif->bt_activity_grading) >= BT_HIGH_TRAFFIC)
                 smps_mode = IEEE80211_SMPS_STATIC;
+        else if (le32_to_cpu(data->notif->bt_activity_grading) >=
+                 BT_LOW_TRAFFIC)
+                smps_mode = IEEE80211_SMPS_DYNAMIC;
 
         IWL_DEBUG_COEX(data->mvm,
-                       "mac %d: bt_status %d traffic_load %d smps_req %d\n",
+                       "mac %d: bt_status %d bt_activity_grading %d smps_req %d\n",
                        mvmvif->id,  data->notif->bt_status,
-                       data->notif->bt_traffic_load, smps_mode);
+                       data->notif->bt_activity_grading, smps_mode);
 
         iwl_mvm_update_smps(mvm, vif, IWL_MVM_SMPS_REQ_BT_COEX, smps_mode);
 
         /* don't reduce the Tx power if in loose scheme */
-        if (is_loose_coex())
+        if (iwl_get_coex_type(mvm, vif) == BT_COEX_LOOSE_LUT ||
+            mvm->cfg->bt_shared_single_ant) {
+                data->reduced_tx_power = false;
+                iwl_mvm_bt_coex_enable_rssi_event(mvm, vif, false, 0);
                 return;
+        }
 
-        data->num_bss_ifaces++;
-
-        /* reduced Txpower only if there are open BT connections, so ...*/
-        if (!BT_MBOX_MSG(data->notif, 3, OPEN_CON_2)) {
+        /* reduced Txpower only if BT is on, so ...*/
+        if (!data->notif->bt_status) {
                 /* ... cancel reduced Tx power ... */
                 if (iwl_mvm_bt_coex_reduced_txp(mvm, mvmvif->ap_sta_id, false))
                         IWL_ERR(mvm, "Couldn't send BT_CONFIG cmd\n");
                 data->reduced_tx_power = false;
 
                 /* ... and there is no need to get reports on RSSI any more. */
-                ieee80211_disable_rssi_reports(vif);
+                iwl_mvm_bt_coex_enable_rssi_event(mvm, vif, false, 0);
                 return;
         }
 
-        ave_rssi = ieee80211_ave_rssi(vif);
+        /* try to get the avg rssi from fw */
+        ave_rssi = mvmvif->bf_data.ave_beacon_signal;
 
         /* if the RSSI isn't valid, fake it is very low */
         if (!ave_rssi)
@@ -499,8 +688,7 @@ static void iwl_mvm_bt_notif_iterator(void *_data, u8 *mac,
         }
 
         /* Begin to monitor the RSSI: it may influence the reduced Tx power */
-        ieee80211_enable_rssi_reports(vif, BT_DISABLE_REDUCED_TXPOWER_THRESHOLD,
-                                      BT_ENABLE_REDUCED_TXPOWER_THRESHOLD);
+        iwl_mvm_bt_coex_enable_rssi_event(mvm, vif, true, ave_rssi);
 }
 
 static void iwl_mvm_bt_coex_notif_handle(struct iwl_mvm *mvm)
@@ -510,11 +698,72 @@ static void iwl_mvm_bt_coex_notif_handle(struct iwl_mvm *mvm)
                 .notif = &mvm->last_bt_notif,
                 .reduced_tx_power = true,
         };
+        struct iwl_bt_coex_ci_cmd cmd = {};
+        u8 ci_bw_idx;
 
+        rcu_read_lock();
         ieee80211_iterate_active_interfaces_atomic(
                                         mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
                                         iwl_mvm_bt_notif_iterator, &data);
 
+        if (data.primary) {
+                struct ieee80211_chanctx_conf *chan = data.primary;
+                if (WARN_ON(!chan->def.chan)) {
+                        rcu_read_unlock();
+                        return;
+                }
+
+                if (chan->def.width < NL80211_CHAN_WIDTH_40) {
+                        ci_bw_idx = 0;
+                        cmd.co_run_bw_primary = 0;
+                } else {
+                        cmd.co_run_bw_primary = 1;
+                        if (chan->def.center_freq1 >
+                            chan->def.chan->center_freq)
+                                ci_bw_idx = 2;
+                        else
+                                ci_bw_idx = 1;
+                }
+
+                cmd.bt_primary_ci =
+                        iwl_ci_mask[chan->def.chan->hw_value][ci_bw_idx];
+                cmd.primary_ch_phy_id = *((u16 *)data.primary->drv_priv);
+        }
+
+        if (data.secondary) {
+                struct ieee80211_chanctx_conf *chan = data.secondary;
+                if (WARN_ON(!data.secondary->def.chan)) {
+                        rcu_read_unlock();
+                        return;
+                }
+
+                if (chan->def.width < NL80211_CHAN_WIDTH_40) {
+                        ci_bw_idx = 0;
+                        cmd.co_run_bw_secondary = 0;
+                } else {
+                        cmd.co_run_bw_secondary = 1;
+                        if (chan->def.center_freq1 >
+                            chan->def.chan->center_freq)
+                                ci_bw_idx = 2;
+                        else
+                                ci_bw_idx = 1;
+                }
+
+                cmd.bt_secondary_ci =
+                        iwl_ci_mask[chan->def.chan->hw_value][ci_bw_idx];
+                cmd.secondary_ch_phy_id = *((u16 *)data.primary->drv_priv);
+        }
+
+        rcu_read_unlock();
+
+        /* Don't spam the fw with the same command over and over */
+        if (memcmp(&cmd, &mvm->last_bt_ci_cmd, sizeof(cmd))) {
+                if (iwl_mvm_send_cmd_pdu(mvm, BT_COEX_CI, CMD_SYNC,
+                                         sizeof(cmd), &cmd))
+                        IWL_ERR(mvm, "Failed to send BT_CI cmd");
+                memcpy(&mvm->last_bt_ci_cmd, &cmd, sizeof(cmd));
+        }
+
         /*
          * If there are no BSS / P2P client interfaces, reduced Tx Power is
          * irrelevant since it is based on the RSSI coming from the beacon.
@@ -536,12 +785,18 @@ int iwl_mvm_rx_bt_coex_notif(struct iwl_mvm *mvm,
 
 
         IWL_DEBUG_COEX(mvm, "BT Coex Notification received\n");
-        IWL_DEBUG_COEX(mvm, "\tBT %salive\n", notif->bt_status ? "" : "not ");
+        IWL_DEBUG_COEX(mvm, "\tBT status: %s\n",
+                       notif->bt_status ? "ON" : "OFF");
         IWL_DEBUG_COEX(mvm, "\tBT open conn %d\n", notif->bt_open_conn);
-        IWL_DEBUG_COEX(mvm, "\tBT traffic load %d\n", notif->bt_traffic_load);
+        IWL_DEBUG_COEX(mvm, "\tBT ci compliance %d\n", notif->bt_ci_compliance);
+        IWL_DEBUG_COEX(mvm, "\tBT primary_ch_lut %d\n",
+                       le32_to_cpu(notif->primary_ch_lut));
+        IWL_DEBUG_COEX(mvm, "\tBT secondary_ch_lut %d\n",
+                       le32_to_cpu(notif->secondary_ch_lut));
+        IWL_DEBUG_COEX(mvm, "\tBT activity grading %d\n",
+                       le32_to_cpu(notif->bt_activity_grading));
         IWL_DEBUG_COEX(mvm, "\tBT agg traffic load %d\n",
                        notif->bt_agg_traffic_load);
-        IWL_DEBUG_COEX(mvm, "\tBT ci compliance %d\n", notif->bt_ci_compliance);
 
         /* remember this notification for future use: rssi fluctuations */
         memcpy(&mvm->last_bt_notif, notif, sizeof(mvm->last_bt_notif));
@@ -565,6 +820,18 @@ static void iwl_mvm_bt_rssi_iterator(void *_data, u8 *mac,
         struct ieee80211_sta *sta;
         struct iwl_mvm_sta *mvmsta;
 
+        struct ieee80211_chanctx_conf *chanctx_conf;
+
+        rcu_read_lock();
+        chanctx_conf = rcu_dereference(vif->chanctx_conf);
+        /* If channel context is invalid or not on 2.4GHz - don't count it */
+        if (!chanctx_conf ||
+            chanctx_conf->def.chan->band != IEEE80211_BAND_2GHZ) {
+                rcu_read_unlock();
+                return;
+        }
+        rcu_read_unlock();
+
         if (vif->type != NL80211_IFTYPE_STATION ||
             mvmvif->ap_sta_id == IWL_MVM_STATION_COUNT)
                 return;
@@ -594,15 +861,15 @@ void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
         };
         int ret;
 
-        mutex_lock(&mvm->mutex);
+        lockdep_assert_held(&mvm->mutex);
 
         /* Rssi update while not associated ?! */
         if (WARN_ON_ONCE(mvmvif->ap_sta_id == IWL_MVM_STATION_COUNT))
-                goto out_unlock;
+                return;
 
-        /* No open connection - reports should be disabled */
-        if (!BT_MBOX_MSG(&mvm->last_bt_notif, 3, OPEN_CON_2))
-                goto out_unlock;
+        /* No BT - reports should be disabled */
+        if (!mvm->last_bt_notif.bt_status)
+                return;
 
         IWL_DEBUG_COEX(mvm, "RSSI for %pM is now %s\n", vif->bss_conf.bssid,
                        rssi_event == RSSI_EVENT_HIGH ? "HIGH" : "LOW");
@@ -611,7 +878,8 @@ void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
          * Check if rssi is good enough for reduced Tx power, but not in loose
          * scheme.
          */
-        if (rssi_event == RSSI_EVENT_LOW || is_loose_coex())
+        if (rssi_event == RSSI_EVENT_LOW || mvm->cfg->bt_shared_single_ant ||
+            iwl_get_coex_type(mvm, vif) == BT_COEX_LOOSE_LUT)
                 ret = iwl_mvm_bt_coex_reduced_txp(mvm, mvmvif->ap_sta_id,
                                                   false);
         else
@@ -633,12 +901,52 @@ void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
         if (iwl_mvm_bt_udpate_ctrl_kill_msk(mvm, data.reduced_tx_power))
                 IWL_ERR(mvm, "Failed to update the ctrl_kill_msk\n");
+}
+
+#define LINK_QUAL_AGG_TIME_LIMIT_DEF    (4000)
+#define LINK_QUAL_AGG_TIME_LIMIT_BT_ACT (1200)
 
- out_unlock:
-        mutex_unlock(&mvm->mutex);
+u16 iwl_mvm_bt_coex_agg_time_limit(struct iwl_mvm *mvm,
+                                   struct ieee80211_sta *sta)
+{
+        struct iwl_mvm_sta *mvmsta = (void *)sta->drv_priv;
+        enum iwl_bt_coex_lut_type lut_type;
+
+        if (le32_to_cpu(mvm->last_bt_notif.bt_activity_grading) <
+            BT_LOW_TRAFFIC)
+                return LINK_QUAL_AGG_TIME_LIMIT_DEF;
+
+        lut_type = iwl_get_coex_type(mvm, mvmsta->vif);
+
+        if (lut_type == BT_COEX_LOOSE_LUT)
+                return LINK_QUAL_AGG_TIME_LIMIT_DEF;
+
+        /* tight coex, high bt traffic, reduce AGG time limit */
+        return LINK_QUAL_AGG_TIME_LIMIT_BT_ACT;
+}
+
+bool iwl_mvm_bt_coex_is_mimo_allowed(struct iwl_mvm *mvm,
+                                     struct ieee80211_sta *sta)
+{
+        struct iwl_mvm_sta *mvmsta = (void *)sta->drv_priv;
+
+        if (le32_to_cpu(mvm->last_bt_notif.bt_activity_grading) <
+            BT_HIGH_TRAFFIC)
+                return true;
+
+        /*
+         * In Tight, BT can't Rx while we Tx, so use both antennas since BT is
+         * already killed.
+         * In Loose, BT can Rx while we Tx, so forbid MIMO to let BT Rx while we
+         * Tx.
+         */
+        return iwl_get_coex_type(mvm, mvmsta->vif) == BT_COEX_TIGHT_LUT;
 }
 
-void iwl_mvm_bt_coex_vif_assoc(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+void iwl_mvm_bt_coex_vif_change(struct iwl_mvm *mvm)
 {
+        if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_NEWBT_COEX))
+                return;
+
         iwl_mvm_bt_coex_notif_handle(mvm);
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/constants.h b/drivers/net/wireless/iwlwifi/mvm/constants.h
index 2bf29f7992ee..4b6d670c3509 100644
--- a/drivers/net/wireless/iwlwifi/mvm/constants.h
+++ b/drivers/net/wireless/iwlwifi/mvm/constants.h
@@ -70,7 +70,9 @@
 #define IWL_MVM_UAPSD_RX_DATA_TIMEOUT           (50 * USEC_PER_MSEC)
 #define IWL_MVM_UAPSD_TX_DATA_TIMEOUT           (50 * USEC_PER_MSEC)
 #define IWL_MVM_PS_HEAVY_TX_THLD_PACKETS        20
-#define IWL_MVM_PS_HEAVY_RX_THLD_PACKETS        20
+#define IWL_MVM_PS_HEAVY_RX_THLD_PACKETS        8
+#define IWL_MVM_PS_SNOOZE_HEAVY_TX_THLD_PACKETS 30
+#define IWL_MVM_PS_SNOOZE_HEAVY_RX_THLD_PACKETS 20
 #define IWL_MVM_PS_HEAVY_TX_THLD_PERCENT        50
 #define IWL_MVM_PS_HEAVY_RX_THLD_PERCENT        50
 #define IWL_MVM_PS_SNOOZE_INTERVAL              25
diff --git a/drivers/net/wireless/iwlwifi/mvm/d3.c b/drivers/net/wireless/iwlwifi/mvm/d3.c
index 417639f77b01..6f45966817bb 100644
--- a/drivers/net/wireless/iwlwifi/mvm/d3.c
+++ b/drivers/net/wireless/iwlwifi/mvm/d3.c
@@ -67,6 +67,7 @@
 #include <net/cfg80211.h>
 #include <net/ipv6.h>
 #include <net/tcp.h>
+#include <net/addrconf.h>
 #include "iwl-modparams.h"
 #include "fw-api.h"
 #include "mvm.h"
@@ -381,14 +382,74 @@ static int iwl_mvm_send_proto_offload(struct iwl_mvm *mvm,
         union {
                 struct iwl_proto_offload_cmd_v1 v1;
                 struct iwl_proto_offload_cmd_v2 v2;
+                struct iwl_proto_offload_cmd_v3_small v3s;
+                struct iwl_proto_offload_cmd_v3_large v3l;
         } cmd = {};
+        struct iwl_host_cmd hcmd = {
+                .id = PROT_OFFLOAD_CONFIG_CMD,
+                .flags = CMD_SYNC,
+                .data[0] = &cmd,
+                .dataflags[0] = IWL_HCMD_DFL_DUP,
+        };
         struct iwl_proto_offload_cmd_common *common;
         u32 enabled = 0, size;
+        u32 capa_flags = mvm->fw->ucode_capa.flags;
 #if IS_ENABLED(CONFIG_IPV6)
         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
         int i;
 
-        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_D3_6_IPV6_ADDRS) {
+        if (capa_flags & IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_SMALL ||
+            capa_flags & IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_LARGE) {
+                struct iwl_ns_config *nsc;
+                struct iwl_targ_addr *addrs;
+                int n_nsc, n_addrs;
+                int c;
+
+                if (capa_flags & IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_SMALL) {
+                        nsc = cmd.v3s.ns_config;
+                        n_nsc = IWL_PROTO_OFFLOAD_NUM_NS_CONFIG_V3S;
+                        addrs = cmd.v3s.targ_addrs;
+                        n_addrs = IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V3S;
+                } else {
+                        nsc = cmd.v3l.ns_config;
+                        n_nsc = IWL_PROTO_OFFLOAD_NUM_NS_CONFIG_V3L;
+                        addrs = cmd.v3l.targ_addrs;
+                        n_addrs = IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V3L;
+                }
+
+                if (mvmvif->num_target_ipv6_addrs)
+                        enabled |= IWL_D3_PROTO_OFFLOAD_NS;
+
+                /*
+                 * For each address we have (and that will fit) fill a target
+                 * address struct and combine for NS offload structs with the
+                 * solicited node addresses.
+                 */
+                for (i = 0, c = 0;
+                     i < mvmvif->num_target_ipv6_addrs &&
+                     i < n_addrs && c < n_nsc; i++) {
+                        struct in6_addr solicited_addr;
+                        int j;
+
+                        addrconf_addr_solict_mult(&mvmvif->target_ipv6_addrs[i],
+                                                  &solicited_addr);
+                        for (j = 0; j < c; j++)
+                                if (ipv6_addr_cmp(&nsc[j].dest_ipv6_addr,
+                                                  &solicited_addr) == 0)
+                                        break;
+                        if (j == c)
+                                c++;
+                        addrs[i].addr = mvmvif->target_ipv6_addrs[i];
+                        addrs[i].config_num = cpu_to_le32(j);
+                        nsc[j].dest_ipv6_addr = solicited_addr;
+                        memcpy(nsc[j].target_mac_addr, vif->addr, ETH_ALEN);
+                }
+
+                if (capa_flags & IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_SMALL)
+                        cmd.v3s.num_valid_ipv6_addrs = cpu_to_le32(i);
+                else
+                        cmd.v3l.num_valid_ipv6_addrs = cpu_to_le32(i);
+        } else if (capa_flags & IWL_UCODE_TLV_FLAGS_D3_6_IPV6_ADDRS) {
                 if (mvmvif->num_target_ipv6_addrs) {
                         enabled |= IWL_D3_PROTO_OFFLOAD_NS;
                         memcpy(cmd.v2.ndp_mac_addr, vif->addr, ETH_ALEN);
@@ -419,7 +480,13 @@ static int iwl_mvm_send_proto_offload(struct iwl_mvm *mvm,
         }
 #endif
 
-        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_D3_6_IPV6_ADDRS) {
+        if (capa_flags & IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_SMALL) {
+                common = &cmd.v3s.common;
+                size = sizeof(cmd.v3s);
+        } else if (capa_flags & IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_LARGE) {
+                common = &cmd.v3l.common;
+                size = sizeof(cmd.v3l);
+        } else if (capa_flags & IWL_UCODE_TLV_FLAGS_D3_6_IPV6_ADDRS) {
                 common = &cmd.v2.common;
                 size = sizeof(cmd.v2);
         } else {
@@ -438,8 +505,8 @@ static int iwl_mvm_send_proto_offload(struct iwl_mvm *mvm,
 
         common->enabled = cpu_to_le32(enabled);
 
-        return iwl_mvm_send_cmd_pdu(mvm, PROT_OFFLOAD_CONFIG_CMD, CMD_SYNC,
-                                    size, &cmd);
+        hcmd.len[0] = size;
+        return iwl_mvm_send_cmd(mvm, &hcmd);
 }
 
 enum iwl_mvm_tcp_packet_type {
@@ -793,6 +860,74 @@ static int iwl_mvm_d3_reprogram(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
         return 0;
 }
 
+static int iwl_mvm_get_last_nonqos_seq(struct iwl_mvm *mvm,
+                                       struct ieee80211_vif *vif)
+{
+        struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+        struct iwl_nonqos_seq_query_cmd query_cmd = {
+                .get_set_flag = cpu_to_le32(IWL_NONQOS_SEQ_GET),
+                .mac_id_n_color =
+                        cpu_to_le32(FW_CMD_ID_AND_COLOR(mvmvif->id,
+                                                        mvmvif->color)),
+        };
+        struct iwl_host_cmd cmd = {
+                .id = NON_QOS_TX_COUNTER_CMD,
+                .flags = CMD_SYNC | CMD_WANT_SKB,
+        };
+        int err;
+        u32 size;
+
+        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_D3_CONTINUITY_API) {
+                cmd.data[0] = &query_cmd;
+                cmd.len[0] = sizeof(query_cmd);
+        }
+
+        err = iwl_mvm_send_cmd(mvm, &cmd);
+        if (err)
+                return err;
+
+        size = le32_to_cpu(cmd.resp_pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
+        size -= sizeof(cmd.resp_pkt->hdr);
+        if (size < sizeof(__le16)) {
+                err = -EINVAL;
+        } else {
+                err = le16_to_cpup((__le16 *)cmd.resp_pkt->data);
+                /* new API returns next, not last-used seqno */
+                if (mvm->fw->ucode_capa.flags &
+                                IWL_UCODE_TLV_FLAGS_D3_CONTINUITY_API)
+                        err -= 0x10;
+        }
+
+        iwl_free_resp(&cmd);
+        return err;
+}
+
+void iwl_mvm_set_last_nonqos_seq(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+{
+        struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+        struct iwl_nonqos_seq_query_cmd query_cmd = {
+                .get_set_flag = cpu_to_le32(IWL_NONQOS_SEQ_SET),
+                .mac_id_n_color =
+                        cpu_to_le32(FW_CMD_ID_AND_COLOR(mvmvif->id,
+                                                        mvmvif->color)),
+                .value = cpu_to_le16(mvmvif->seqno),
+        };
+
+        /* return if called during restart, not resume from D3 */
+        if (!mvmvif->seqno_valid)
+                return;
+
+        mvmvif->seqno_valid = false;
+
+        if (!(mvm->fw->ucode_capa.flags &
+                        IWL_UCODE_TLV_FLAGS_D3_CONTINUITY_API))
+                return;
+
+        if (iwl_mvm_send_cmd_pdu(mvm, NON_QOS_TX_COUNTER_CMD, CMD_SYNC,
+                                 sizeof(query_cmd), &query_cmd))
+                IWL_ERR(mvm, "failed to set non-QoS seqno\n");
+}
+
 static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
                              struct cfg80211_wowlan *wowlan,
                              bool test)
@@ -829,7 +964,6 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
         };
         int ret, i;
         int len __maybe_unused;
-        u16 seq;
         u8 old_aux_sta_id, old_ap_sta_id = IWL_MVM_STATION_COUNT;
 
         if (!wowlan) {
@@ -872,26 +1006,15 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
 
         mvm_ap_sta = (struct iwl_mvm_sta *)ap_sta->drv_priv;
 
-        /*
-         * The D3 firmware still hardcodes the AP station ID for the
-         * BSS we're associated with as 0. Store the real STA ID here
-         * and assign 0. When we leave this function, we'll restore
-         * the original value for the resume code.
-         */
-        old_ap_sta_id = mvm_ap_sta->sta_id;
-        mvm_ap_sta->sta_id = 0;
-        mvmvif->ap_sta_id = 0;
-
         /* TODO: wowlan_config_cmd.wowlan_ba_teardown_tids */
 
         wowlan_config_cmd.is_11n_connection = ap_sta->ht_cap.ht_supported;
 
-        /*
-         * We know the last used seqno, and the uCode expects to know that
-         * one, it will increment before TX.
-         */
-        seq = mvm_ap_sta->last_seq_ctl & IEEE80211_SCTL_SEQ;
-        wowlan_config_cmd.non_qos_seq = cpu_to_le16(seq);
+        /* Query the last used seqno and set it */
+        ret = iwl_mvm_get_last_nonqos_seq(mvm, vif);
+        if (ret < 0)
+                goto out_noreset;
+        wowlan_config_cmd.non_qos_seq = cpu_to_le16(ret);
 
         /*
          * For QoS counters, we store the one to use next, so subtract 0x10
@@ -899,7 +1022,7 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
          * increment after using the value (i.e. store the next value to use).
          */
         for (i = 0; i < IWL_MAX_TID_COUNT; i++) {
-                seq = mvm_ap_sta->tid_data[i].seq_number;
+                u16 seq = mvm_ap_sta->tid_data[i].seq_number;
                 seq -= 0x10;
                 wowlan_config_cmd.qos_seq[i] = cpu_to_le16(seq);
         }
@@ -945,6 +1068,16 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
         iwl_trans_stop_device(mvm->trans);
 
         /*
+         * The D3 firmware still hardcodes the AP station ID for the
+         * BSS we're associated with as 0. Store the real STA ID here
+         * and assign 0. When we leave this function, we'll restore
+         * the original value for the resume code.
+         */
+        old_ap_sta_id = mvm_ap_sta->sta_id;
+        mvm_ap_sta->sta_id = 0;
+        mvmvif->ap_sta_id = 0;
+
+        /*
          * Set the HW restart bit -- this is mostly true as we're
          * going to load new firmware and reprogram that, though
          * the reprogramming is going to be manual to avoid adding
@@ -1059,6 +1192,10 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
         if (ret)
                 goto out;
 
+        ret = iwl_mvm_power_update_device_mode(mvm);
+        if (ret)
+                goto out;
+
         ret = iwl_mvm_power_update_mode(mvm, vif);
         if (ret)
                 goto out;
@@ -1109,16 +1246,26 @@ int iwl_mvm_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
         return __iwl_mvm_suspend(hw, wowlan, false);
 }
 
+/* converted data from the different status responses */
+struct iwl_wowlan_status_data {
+        u16 pattern_number;
+        u16 qos_seq_ctr[8];
+        u32 wakeup_reasons;
+        u32 wake_packet_length;
+        u32 wake_packet_bufsize;
+        const u8 *wake_packet;
+};
+
 static void iwl_mvm_report_wakeup_reasons(struct iwl_mvm *mvm,
                                           struct ieee80211_vif *vif,
-                                          struct iwl_wowlan_status *status)
+                                          struct iwl_wowlan_status_data *status)
 {
         struct sk_buff *pkt = NULL;
         struct cfg80211_wowlan_wakeup wakeup = {
                 .pattern_idx = -1,
         };
         struct cfg80211_wowlan_wakeup *wakeup_report = &wakeup;
-        u32 reasons = le32_to_cpu(status->wakeup_reasons);
+        u32 reasons = status->wakeup_reasons;
 
         if (reasons == IWL_WOWLAN_WAKEUP_BY_NON_WIRELESS) {
                 wakeup_report = NULL;
@@ -1130,7 +1277,7 @@ static void iwl_mvm_report_wakeup_reasons(struct iwl_mvm *mvm,
 
         if (reasons & IWL_WOWLAN_WAKEUP_BY_PATTERN)
                 wakeup.pattern_idx =
-                        le16_to_cpu(status->pattern_number);
+                        status->pattern_number;
 
         if (reasons & (IWL_WOWLAN_WAKEUP_BY_DISCONNECTION_ON_MISSED_BEACON |
                        IWL_WOWLAN_WAKEUP_BY_DISCONNECTION_ON_DEAUTH))
@@ -1158,8 +1305,8 @@ static void iwl_mvm_report_wakeup_reasons(struct iwl_mvm *mvm,
                 wakeup.tcp_match = true;
 
         if (status->wake_packet_bufsize) {
-                int pktsize = le32_to_cpu(status->wake_packet_bufsize);
-                int pktlen = le32_to_cpu(status->wake_packet_length);
+                int pktsize = status->wake_packet_bufsize;
+                int pktlen = status->wake_packet_length;
                 const u8 *pktdata = status->wake_packet;
                 struct ieee80211_hdr *hdr = (void *)pktdata;
                 int truncated = pktlen - pktsize;
@@ -1239,8 +1386,229 @@ static void iwl_mvm_report_wakeup_reasons(struct iwl_mvm *mvm,
         kfree_skb(pkt);
 }
 
+static void iwl_mvm_aes_sc_to_seq(struct aes_sc *sc,
+                                  struct ieee80211_key_seq *seq)
+{
+        u64 pn;
+
+        pn = le64_to_cpu(sc->pn);
+        seq->ccmp.pn[0] = pn >> 40;
+        seq->ccmp.pn[1] = pn >> 32;
+        seq->ccmp.pn[2] = pn >> 24;
+        seq->ccmp.pn[3] = pn >> 16;
+        seq->ccmp.pn[4] = pn >> 8;
+        seq->ccmp.pn[5] = pn;
+}
+
+static void iwl_mvm_tkip_sc_to_seq(struct tkip_sc *sc,
+                                   struct ieee80211_key_seq *seq)
+{
+        seq->tkip.iv32 = le32_to_cpu(sc->iv32);
+        seq->tkip.iv16 = le16_to_cpu(sc->iv16);
+}
+
+static void iwl_mvm_set_aes_rx_seq(struct aes_sc *scs,
+                                   struct ieee80211_key_conf *key)
+{
+        int tid;
+
+        BUILD_BUG_ON(IWL_NUM_RSC != IEEE80211_NUM_TIDS);
+
+        for (tid = 0; tid < IWL_NUM_RSC; tid++) {
+                struct ieee80211_key_seq seq = {};
+
+                iwl_mvm_aes_sc_to_seq(&scs[tid], &seq);
+                ieee80211_set_key_rx_seq(key, tid, &seq);
+        }
+}
+
+static void iwl_mvm_set_tkip_rx_seq(struct tkip_sc *scs,
+                                    struct ieee80211_key_conf *key)
+{
+        int tid;
+
+        BUILD_BUG_ON(IWL_NUM_RSC != IEEE80211_NUM_TIDS);
+
+        for (tid = 0; tid < IWL_NUM_RSC; tid++) {
+                struct ieee80211_key_seq seq = {};
+
+                iwl_mvm_tkip_sc_to_seq(&scs[tid], &seq);
+                ieee80211_set_key_rx_seq(key, tid, &seq);
+        }
+}
+
+static void iwl_mvm_set_key_rx_seq(struct ieee80211_key_conf *key,
+                                   struct iwl_wowlan_status_v6 *status)
+{
+        union iwl_all_tsc_rsc *rsc = &status->gtk.rsc.all_tsc_rsc;
+
+        switch (key->cipher) {
+        case WLAN_CIPHER_SUITE_CCMP:
+                iwl_mvm_set_aes_rx_seq(rsc->aes.multicast_rsc, key);
+                break;
+        case WLAN_CIPHER_SUITE_TKIP:
+                iwl_mvm_set_tkip_rx_seq(rsc->tkip.multicast_rsc, key);
+                break;
+        default:
+                WARN_ON(1);
+        }
+}
+
+struct iwl_mvm_d3_gtk_iter_data {
+        struct iwl_wowlan_status_v6 *status;
+        void *last_gtk;
+        u32 cipher;
+        bool find_phase, unhandled_cipher;
+        int num_keys;
+};
+
+static void iwl_mvm_d3_update_gtks(struct ieee80211_hw *hw,
+                                   struct ieee80211_vif *vif,
+                                   struct ieee80211_sta *sta,
+                                   struct ieee80211_key_conf *key,
+                                   void *_data)
+{
+        struct iwl_mvm_d3_gtk_iter_data *data = _data;
+
+        if (data->unhandled_cipher)
+                return;
+
+        switch (key->cipher) {
+        case WLAN_CIPHER_SUITE_WEP40:
+        case WLAN_CIPHER_SUITE_WEP104:
+                /* ignore WEP completely, nothing to do */
+                return;
+        case WLAN_CIPHER_SUITE_CCMP:
+        case WLAN_CIPHER_SUITE_TKIP:
+                /* we support these */
+                break;
+        default:
+                /* everything else (even CMAC for MFP) - disconnect from AP */
+                data->unhandled_cipher = true;
+                return;
+        }
+
+        data->num_keys++;
+
+        /*
+         * pairwise key - update sequence counters only;
+         * note that this assumes no TDLS sessions are active
+         */
+        if (sta) {
+                struct ieee80211_key_seq seq = {};
+                union iwl_all_tsc_rsc *sc = &data->status->gtk.rsc.all_tsc_rsc;
+
+                if (data->find_phase)
+                        return;
+
+                switch (key->cipher) {
+                case WLAN_CIPHER_SUITE_CCMP:
+                        iwl_mvm_aes_sc_to_seq(&sc->aes.tsc, &seq);
+                        iwl_mvm_set_aes_rx_seq(sc->aes.unicast_rsc, key);
+                        break;
+                case WLAN_CIPHER_SUITE_TKIP:
+                        iwl_mvm_tkip_sc_to_seq(&sc->tkip.tsc, &seq);
+                        iwl_mvm_set_tkip_rx_seq(sc->tkip.unicast_rsc, key);
+                        break;
+                }
+                ieee80211_set_key_tx_seq(key, &seq);
+
+                /* that's it for this key */
+                return;
+        }
+
+        if (data->find_phase) {
+                data->last_gtk = key;
+                data->cipher = key->cipher;
+                return;
+        }
+
+        if (data->status->num_of_gtk_rekeys)
+                ieee80211_remove_key(key);
+        else if (data->last_gtk == key)
+                iwl_mvm_set_key_rx_seq(key, data->status);
+}
+
+static bool iwl_mvm_setup_connection_keep(struct iwl_mvm *mvm,
+                                          struct ieee80211_vif *vif,
+                                          struct iwl_wowlan_status_v6 *status)
+{
+        struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+        struct iwl_mvm_d3_gtk_iter_data gtkdata = {
+                .status = status,
+        };
+
+        if (!status || !vif->bss_conf.bssid)
+                return false;
+
+        /* find last GTK that we used initially, if any */
+        gtkdata.find_phase = true;
+        ieee80211_iter_keys(mvm->hw, vif,
+                            iwl_mvm_d3_update_gtks, &gtkdata);
+        /* not trying to keep connections with MFP/unhandled ciphers */
+        if (gtkdata.unhandled_cipher)
+                return false;
+        if (!gtkdata.num_keys)
+                return true;
+        if (!gtkdata.last_gtk)
+                return false;
+
+        /*
+         * invalidate all other GTKs that might still exist and update
+         * the one that we used
+         */
+        gtkdata.find_phase = false;
+        ieee80211_iter_keys(mvm->hw, vif,
+                            iwl_mvm_d3_update_gtks, &gtkdata);
+
+        if (status->num_of_gtk_rekeys) {
+                struct ieee80211_key_conf *key;
+                struct {
+                        struct ieee80211_key_conf conf;
+                        u8 key[32];
+                } conf = {
+                        .conf.cipher = gtkdata.cipher,
+                        .conf.keyidx = status->gtk.key_index,
+                };
+
+                switch (gtkdata.cipher) {
+                case WLAN_CIPHER_SUITE_CCMP:
+                        conf.conf.keylen = WLAN_KEY_LEN_CCMP;
+                        memcpy(conf.conf.key, status->gtk.decrypt_key,
+                               WLAN_KEY_LEN_CCMP);
+                        break;
+                case WLAN_CIPHER_SUITE_TKIP:
+                        conf.conf.keylen = WLAN_KEY_LEN_TKIP;
+                        memcpy(conf.conf.key, status->gtk.decrypt_key, 16);
+                        /* leave TX MIC key zeroed, we don't use it anyway */
+                        memcpy(conf.conf.key +
+                               NL80211_TKIP_DATA_OFFSET_RX_MIC_KEY,
+                               status->gtk.tkip_mic_key, 8);
+                        break;
+                }
+
+                key = ieee80211_gtk_rekey_add(vif, &conf.conf);
+                if (IS_ERR(key))
+                        return false;
+                iwl_mvm_set_key_rx_seq(key, status);
+        }
+
+        if (status->num_of_gtk_rekeys) {
+                __be64 replay_ctr =
+                        cpu_to_be64(le64_to_cpu(status->replay_ctr));
+                ieee80211_gtk_rekey_notify(vif, vif->bss_conf.bssid,
+                                           (void *)&replay_ctr, GFP_KERNEL);
+        }
+
+        mvmvif->seqno_valid = true;
+        /* +0x10 because the set API expects next-to-use, not last-used */
+        mvmvif->seqno = le16_to_cpu(status->non_qos_seq_ctr) + 0x10;
+
+        return true;
+}
+
 /* releases the MVM mutex */
-static void iwl_mvm_query_wakeup_reasons(struct iwl_mvm *mvm,
+static bool iwl_mvm_query_wakeup_reasons(struct iwl_mvm *mvm,
                                          struct ieee80211_vif *vif)
 {
         u32 base = mvm->error_event_table;
@@ -1253,8 +1621,12 @@ static void iwl_mvm_query_wakeup_reasons(struct iwl_mvm *mvm,
                 .id = WOWLAN_GET_STATUSES,
                 .flags = CMD_SYNC | CMD_WANT_SKB,
         };
-        struct iwl_wowlan_status *status;
-        int ret, len;
+        struct iwl_wowlan_status_data status;
+        struct iwl_wowlan_status_v6 *status_v6;
+        int ret, len, status_size, i;
+        bool keep;
+        struct ieee80211_sta *ap_sta;
+        struct iwl_mvm_sta *mvm_ap_sta;
 
         iwl_trans_read_mem_bytes(mvm->trans, base,
                                  &err_info, sizeof(err_info));
@@ -1287,32 +1659,83 @@ static void iwl_mvm_query_wakeup_reasons(struct iwl_mvm *mvm,
         if (!cmd.resp_pkt)
                 goto out_unlock;
 
+        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_D3_CONTINUITY_API)
+                status_size = sizeof(struct iwl_wowlan_status_v6);
+        else
+                status_size = sizeof(struct iwl_wowlan_status_v4);
+
         len = le32_to_cpu(cmd.resp_pkt->len_n_flags) & FH_RSCSR_FRAME_SIZE_MSK;
-        if (len - sizeof(struct iwl_cmd_header) < sizeof(*status)) {
+        if (len - sizeof(struct iwl_cmd_header) < status_size) {
                 IWL_ERR(mvm, "Invalid WoWLAN status response!\n");
                 goto out_free_resp;
         }
 
-        status = (void *)cmd.resp_pkt->data;
+        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_D3_CONTINUITY_API) {
+                status_v6 = (void *)cmd.resp_pkt->data;
+
+                status.pattern_number = le16_to_cpu(status_v6->pattern_number);
+                for (i = 0; i < 8; i++)
+                        status.qos_seq_ctr[i] =
+                                le16_to_cpu(status_v6->qos_seq_ctr[i]);
+                status.wakeup_reasons = le32_to_cpu(status_v6->wakeup_reasons);
+                status.wake_packet_length =
+                        le32_to_cpu(status_v6->wake_packet_length);
+                status.wake_packet_bufsize =
+                        le32_to_cpu(status_v6->wake_packet_bufsize);
+                status.wake_packet = status_v6->wake_packet;
+        } else {
+                struct iwl_wowlan_status_v4 *status_v4;
+                status_v6 = NULL;
+                status_v4 = (void *)cmd.resp_pkt->data;
+
+                status.pattern_number = le16_to_cpu(status_v4->pattern_number);
+                for (i = 0; i < 8; i++)
+                        status.qos_seq_ctr[i] =
+                                le16_to_cpu(status_v4->qos_seq_ctr[i]);
+                status.wakeup_reasons = le32_to_cpu(status_v4->wakeup_reasons);
+                status.wake_packet_length =
+                        le32_to_cpu(status_v4->wake_packet_length);
+                status.wake_packet_bufsize =
+                        le32_to_cpu(status_v4->wake_packet_bufsize);
+                status.wake_packet = status_v4->wake_packet;
+        }
 
         if (len - sizeof(struct iwl_cmd_header) !=
-            sizeof(*status) +
-            ALIGN(le32_to_cpu(status->wake_packet_bufsize), 4)) {
+            status_size + ALIGN(status.wake_packet_bufsize, 4)) {
                 IWL_ERR(mvm, "Invalid WoWLAN status response!\n");
                 goto out_free_resp;
         }
 
+        /* still at hard-coded place 0 for D3 image */
+        ap_sta = rcu_dereference_protected(
+                        mvm->fw_id_to_mac_id[0],
+                        lockdep_is_held(&mvm->mutex));
+        if (IS_ERR_OR_NULL(ap_sta))
+                goto out_free_resp;
+
+        mvm_ap_sta = (struct iwl_mvm_sta *)ap_sta->drv_priv;
+        for (i = 0; i < IWL_MAX_TID_COUNT; i++) {
+                u16 seq = status.qos_seq_ctr[i];
+                /* firmware stores last-used value, we store next value */
+                seq += 0x10;
+                mvm_ap_sta->tid_data[i].seq_number = seq;
+        }
+
         /* now we have all the data we need, unlock to avoid mac80211 issues */
         mutex_unlock(&mvm->mutex);
 
-        iwl_mvm_report_wakeup_reasons(mvm, vif, status);
+        iwl_mvm_report_wakeup_reasons(mvm, vif, &status);
+
+        keep = iwl_mvm_setup_connection_keep(mvm, vif, status_v6);
+
         iwl_free_resp(&cmd);
-        return;
+        return keep;
 
  out_free_resp:
         iwl_free_resp(&cmd);
  out_unlock:
         mutex_unlock(&mvm->mutex);
+        return false;
 }
 
 static void iwl_mvm_read_d3_sram(struct iwl_mvm *mvm)
@@ -1335,6 +1758,17 @@ static void iwl_mvm_read_d3_sram(struct iwl_mvm *mvm)
 #endif
 }
 
+static void iwl_mvm_d3_disconnect_iter(void *data, u8 *mac,
+                                       struct ieee80211_vif *vif)
+{
+        /* skip the one we keep connection on */
+        if (data == vif)
+                return;
+
+        if (vif->type == NL80211_IFTYPE_STATION)
+                ieee80211_resume_disconnect(vif);
+}
+
 static int __iwl_mvm_resume(struct iwl_mvm *mvm, bool test)
 {
         struct iwl_d3_iter_data resume_iter_data = {
@@ -1343,6 +1777,7 @@ static int __iwl_mvm_resume(struct iwl_mvm *mvm, bool test)
         struct ieee80211_vif *vif = NULL;
         int ret;
         enum iwl_d3_status d3_status;
+        bool keep = false;
 
         mutex_lock(&mvm->mutex);
 
@@ -1368,7 +1803,7 @@ static int __iwl_mvm_resume(struct iwl_mvm *mvm, bool test)
         /* query SRAM first in case we want event logging */
         iwl_mvm_read_d3_sram(mvm);
 
-        iwl_mvm_query_wakeup_reasons(mvm, vif);
+        keep = iwl_mvm_query_wakeup_reasons(mvm, vif);
         /* has unlocked the mutex, so skip that */
         goto out;
 
@@ -1376,8 +1811,10 @@ static int __iwl_mvm_resume(struct iwl_mvm *mvm, bool test)
         mutex_unlock(&mvm->mutex);
 
  out:
-        if (!test && vif)
-                ieee80211_resume_disconnect(vif);
+        if (!test)
+                ieee80211_iterate_active_interfaces_rtnl(mvm->hw,
+                        IEEE80211_IFACE_ITER_NORMAL,
+                        iwl_mvm_d3_disconnect_iter, keep ? vif : NULL);
 
         /* return 1 to reconfigure the device */
         set_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status);
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs.c b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
index aac81b8984b0..0675f0c8ef93 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
@@ -246,58 +246,56 @@ static ssize_t iwl_dbgfs_stations_read(struct file *file, char __user *user_buf,
         return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
 
-static ssize_t iwl_dbgfs_power_down_allow_write(struct file *file,
-                                                const char __user *user_buf,
+static ssize_t iwl_dbgfs_disable_power_off_read(struct file *file,
+                                                char __user *user_buf,
                                                 size_t count, loff_t *ppos)
 {
         struct iwl_mvm *mvm = file->private_data;
-        char buf[8] = {};
-        int allow;
-
-        if (!mvm->ucode_loaded)
-                return -EIO;
-
-        if (copy_from_user(buf, user_buf, sizeof(buf)))
-                return -EFAULT;
-
-        if (sscanf(buf, "%d", &allow) != 1)
-                return -EINVAL;
-
-        IWL_DEBUG_POWER(mvm, "%s device power down\n",
-                        allow ? "allow" : "prevent");
+        char buf[64];
+        int bufsz = sizeof(buf);
+        int pos = 0;
 
-        /*
-         * TODO: Send REPLY_DEBUG_CMD (0xf0) when FW support it
-         */
+        pos += scnprintf(buf+pos, bufsz-pos, "disable_power_off_d0=%d\n",
+                         mvm->disable_power_off);
+        pos += scnprintf(buf+pos, bufsz-pos, "disable_power_off_d3=%d\n",
+                         mvm->disable_power_off_d3);
 
-        return count;
+        return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
 
-static ssize_t iwl_dbgfs_power_down_d3_allow_write(struct file *file,
-                                                   const char __user *user_buf,
-                                                   size_t count, loff_t *ppos)
+static ssize_t iwl_dbgfs_disable_power_off_write(struct file *file,
+                                                 const char __user *user_buf,
+                                                 size_t count, loff_t *ppos)
 {
         struct iwl_mvm *mvm = file->private_data;
-        char buf[8] = {};
-        int allow;
+        char buf[64] = {};
+        int ret;
+        int val;
 
-        if (copy_from_user(buf, user_buf, sizeof(buf)))
+        if (!mvm->ucode_loaded)
+                return -EIO;
+
+        count = min_t(size_t, count, sizeof(buf) - 1);
+        if (copy_from_user(buf, user_buf, count))
                 return -EFAULT;
 
-        if (sscanf(buf, "%d", &allow) != 1)
+        if (!strncmp("disable_power_off_d0=", buf, 21)) {
+                if (sscanf(buf + 21, "%d", &val) != 1)
+                        return -EINVAL;
+                mvm->disable_power_off = val;
+        } else if (!strncmp("disable_power_off_d3=", buf, 21)) {
+                if (sscanf(buf + 21, "%d", &val) != 1)
+                        return -EINVAL;
+                mvm->disable_power_off_d3 = val;
+        } else {
                 return -EINVAL;
+        }
 
-        IWL_DEBUG_POWER(mvm, "%s device power down in d3\n",
-                        allow ? "allow" : "prevent");
-
-        /*
-         * TODO: When WoWLAN FW alive notification happens, driver will send
-         * REPLY_DEBUG_CMD setting power_down_allow flag according to
-         * mvm->prevent_power_down_d3
-         */
-        mvm->prevent_power_down_d3 = !allow;
+        mutex_lock(&mvm->mutex);
+        ret = iwl_mvm_power_update_device_mode(mvm);
+        mutex_unlock(&mvm->mutex);
 
-        return count;
+        return ret ?: count;
 }
 
 static void iwl_dbgfs_update_pm(struct iwl_mvm *mvm,
@@ -371,7 +369,8 @@ static ssize_t iwl_dbgfs_pm_params_write(struct file *file,
         int val;
         int ret;
 
-        if (copy_from_user(buf, user_buf, sizeof(buf)))
+        count = min_t(size_t, count, sizeof(buf) - 1);
+        if (copy_from_user(buf, user_buf, count))
                 return -EFAULT;
 
         if (!strncmp("keep_alive=", buf, 11)) {
@@ -394,7 +393,9 @@ static ssize_t iwl_dbgfs_pm_params_write(struct file *file,
                 if (sscanf(buf + 16, "%d", &val) != 1)
                         return -EINVAL;
                 param = MVM_DEBUGFS_PM_TX_DATA_TIMEOUT;
-        } else if (!strncmp("disable_power_off=", buf, 18)) {
+        } else if (!strncmp("disable_power_off=", buf, 18) &&
+                   !(mvm->fw->ucode_capa.flags &
+                     IWL_UCODE_TLV_FLAGS_DEVICE_PS_CMD)) {
                 if (sscanf(buf + 18, "%d", &val) != 1)
                         return -EINVAL;
                 param = MVM_DEBUGFS_PM_DISABLE_POWER_OFF;
@@ -581,15 +582,21 @@ static ssize_t iwl_dbgfs_bt_notif_read(struct file *file, char __user *user_buf,
         BT_MBOX_PRINT(3, UPDATE_REQUEST, true);
 
         pos += scnprintf(buf+pos, bufsz-pos, "bt_status = %d\n",
-                                         notif->bt_status);
+                         notif->bt_status);
         pos += scnprintf(buf+pos, bufsz-pos, "bt_open_conn = %d\n",
-                                         notif->bt_open_conn);
+                         notif->bt_open_conn);
         pos += scnprintf(buf+pos, bufsz-pos, "bt_traffic_load = %d\n",
-                                         notif->bt_traffic_load);
+                         notif->bt_traffic_load);
         pos += scnprintf(buf+pos, bufsz-pos, "bt_agg_traffic_load = %d\n",
-                                         notif->bt_agg_traffic_load);
+                         notif->bt_agg_traffic_load);
         pos += scnprintf(buf+pos, bufsz-pos, "bt_ci_compliance = %d\n",
-                                         notif->bt_ci_compliance);
+                         notif->bt_ci_compliance);
+        pos += scnprintf(buf+pos, bufsz-pos, "primary_ch_lut = %d\n",
+                         le32_to_cpu(notif->primary_ch_lut));
+        pos += scnprintf(buf+pos, bufsz-pos, "secondary_ch_lut = %d\n",
+                         le32_to_cpu(notif->secondary_ch_lut));
+        pos += scnprintf(buf+pos, bufsz-pos, "bt_activity_grading = %d\n",
+                         le32_to_cpu(notif->bt_activity_grading));
 
         mutex_unlock(&mvm->mutex);
 
@@ -600,6 +607,38 @@ static ssize_t iwl_dbgfs_bt_notif_read(struct file *file, char __user *user_buf,
 }
 #undef BT_MBOX_PRINT
 
+static ssize_t iwl_dbgfs_bt_cmd_read(struct file *file, char __user *user_buf,
+                                     size_t count, loff_t *ppos)
+{
+        struct iwl_mvm *mvm = file->private_data;
+        struct iwl_bt_coex_ci_cmd *cmd = &mvm->last_bt_ci_cmd;
+        char buf[256];
+        int bufsz = sizeof(buf);
+        int pos = 0;
+
+        mutex_lock(&mvm->mutex);
+
+        pos += scnprintf(buf+pos, bufsz-pos, "Channel inhibition CMD\n");
+        pos += scnprintf(buf+pos, bufsz-pos,
+                       "\tPrimary Channel Bitmap 0x%016llx Fat: %d\n",
+                       le64_to_cpu(cmd->bt_primary_ci),
+                       !!cmd->co_run_bw_primary);
+        pos += scnprintf(buf+pos, bufsz-pos,
+                       "\tSecondary Channel Bitmap 0x%016llx Fat: %d\n",
+                       le64_to_cpu(cmd->bt_secondary_ci),
+                       !!cmd->co_run_bw_secondary);
+
+        pos += scnprintf(buf+pos, bufsz-pos, "BT Configuration CMD\n");
+        pos += scnprintf(buf+pos, bufsz-pos, "\tACK Kill Mask 0x%08x\n",
+                         iwl_bt_ack_kill_msk[mvm->bt_kill_msk]);
+        pos += scnprintf(buf+pos, bufsz-pos, "\tCTS Kill Mask 0x%08x\n",
+                         iwl_bt_cts_kill_msk[mvm->bt_kill_msk]);
+
+        mutex_unlock(&mvm->mutex);
+
+        return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
 #define PRINT_STATS_LE32(_str, _val)                                    \
                          pos += scnprintf(buf + pos, bufsz - pos,       \
                                           fmt_table, _str,              \
@@ -615,9 +654,11 @@ static ssize_t iwl_dbgfs_fw_rx_stats_read(struct file *file,
         int pos = 0;
         char *buf;
         int ret;
-        int bufsz = sizeof(struct mvm_statistics_rx_phy) * 20 +
-                    sizeof(struct mvm_statistics_rx_non_phy) * 10 +
-                    sizeof(struct mvm_statistics_rx_ht_phy) * 10 + 200;
+        /* 43 is the size of each data line, 33 is the size of each header */
+        size_t bufsz =
+                ((sizeof(struct mvm_statistics_rx) / sizeof(__le32)) * 43) +
+                (4 * 33) + 1;
+
         struct mvm_statistics_rx_phy *ofdm;
         struct mvm_statistics_rx_phy *cck;
         struct mvm_statistics_rx_non_phy *general;
@@ -712,6 +753,7 @@ static ssize_t iwl_dbgfs_fw_rx_stats_read(struct file *file,
         PRINT_STATS_LE32("beacon_energy_b", general->beacon_energy_b);
         PRINT_STATS_LE32("beacon_energy_c", general->beacon_energy_c);
         PRINT_STATS_LE32("num_bt_kills", general->num_bt_kills);
+        PRINT_STATS_LE32("mac_id", general->mac_id);
         PRINT_STATS_LE32("directed_data_mpdu", general->directed_data_mpdu);
 
         pos += scnprintf(buf + pos, bufsz - pos, fmt_header,
@@ -757,6 +799,59 @@ static ssize_t iwl_dbgfs_fw_restart_write(struct file *file,
         return count;
 }
 
+static ssize_t
+iwl_dbgfs_scan_ant_rxchain_read(struct file *file,
+                                char __user *user_buf,
+                                size_t count, loff_t *ppos)
+{
+        struct iwl_mvm *mvm = file->private_data;
+        int pos = 0;
+        char buf[32];
+        const size_t bufsz = sizeof(buf);
+
+        /* print which antennas were set for the scan command by the user */
+        pos += scnprintf(buf + pos, bufsz - pos, "Antennas for scan: ");
+        if (mvm->scan_rx_ant & ANT_A)
+                pos += scnprintf(buf + pos, bufsz - pos, "A");
+        if (mvm->scan_rx_ant & ANT_B)
+                pos += scnprintf(buf + pos, bufsz - pos, "B");
+        if (mvm->scan_rx_ant & ANT_C)
+                pos += scnprintf(buf + pos, bufsz - pos, "C");
+        pos += scnprintf(buf + pos, bufsz - pos, " (%hhx)\n", mvm->scan_rx_ant);
+
+        return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+static ssize_t
+iwl_dbgfs_scan_ant_rxchain_write(struct file *file,
+                                 const char __user *user_buf,
+                                 size_t count, loff_t *ppos)
+{
+        struct iwl_mvm *mvm = file->private_data;
+        char buf[8];
+        int buf_size;
+        u8 scan_rx_ant;
+
+        memset(buf, 0, sizeof(buf));
+        buf_size = min(count, sizeof(buf) - 1);
+
+        /* get the argument from the user and check if it is valid */
+        if (copy_from_user(buf, user_buf, buf_size))
+                return -EFAULT;
+        if (sscanf(buf, "%hhx", &scan_rx_ant) != 1)
+                return -EINVAL;
+        if (scan_rx_ant > ANT_ABC)
+                return -EINVAL;
+        if (scan_rx_ant & ~iwl_fw_valid_rx_ant(mvm->fw))
+                return -EINVAL;
+
+        /* change the rx antennas for scan command */
+        mvm->scan_rx_ant = scan_rx_ant;
+
+        return count;
+}
+
+
 static void iwl_dbgfs_update_bf(struct ieee80211_vif *vif,
                                 enum iwl_dbgfs_bf_mask param, int value)
 {
@@ -968,7 +1063,8 @@ static ssize_t iwl_dbgfs_d3_sram_write(struct file *file,
         char buf[8] = {};
         int store;
 
-        if (copy_from_user(buf, user_buf, sizeof(buf)))
+        count = min_t(size_t, count, sizeof(buf) - 1);
+        if (copy_from_user(buf, user_buf, count))
                 return -EFAULT;
 
         if (sscanf(buf, "%d", &store) != 1)
@@ -1063,10 +1159,12 @@ MVM_DEBUGFS_WRITE_FILE_OPS(sta_drain);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(sram);
 MVM_DEBUGFS_READ_FILE_OPS(stations);
 MVM_DEBUGFS_READ_FILE_OPS(bt_notif);
-MVM_DEBUGFS_WRITE_FILE_OPS(power_down_allow);
-MVM_DEBUGFS_WRITE_FILE_OPS(power_down_d3_allow);
+MVM_DEBUGFS_READ_FILE_OPS(bt_cmd);
+MVM_DEBUGFS_READ_WRITE_FILE_OPS(disable_power_off);
 MVM_DEBUGFS_READ_FILE_OPS(fw_rx_stats);
 MVM_DEBUGFS_WRITE_FILE_OPS(fw_restart);
+MVM_DEBUGFS_READ_WRITE_FILE_OPS(scan_ant_rxchain);
+
 #ifdef CONFIG_PM_SLEEP
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(d3_sram);
 #endif
@@ -1087,10 +1185,14 @@ int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
         MVM_DEBUGFS_ADD_FILE(sram, mvm->debugfs_dir, S_IWUSR | S_IRUSR);
         MVM_DEBUGFS_ADD_FILE(stations, dbgfs_dir, S_IRUSR);
         MVM_DEBUGFS_ADD_FILE(bt_notif, dbgfs_dir, S_IRUSR);
-        MVM_DEBUGFS_ADD_FILE(power_down_allow, mvm->debugfs_dir, S_IWUSR);
-        MVM_DEBUGFS_ADD_FILE(power_down_d3_allow, mvm->debugfs_dir, S_IWUSR);
+        MVM_DEBUGFS_ADD_FILE(bt_cmd, dbgfs_dir, S_IRUSR);
+        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_DEVICE_PS_CMD)
+                MVM_DEBUGFS_ADD_FILE(disable_power_off, mvm->debugfs_dir,
+                                     S_IRUSR | S_IWUSR);
         MVM_DEBUGFS_ADD_FILE(fw_rx_stats, mvm->debugfs_dir, S_IRUSR);
         MVM_DEBUGFS_ADD_FILE(fw_restart, mvm->debugfs_dir, S_IWUSR);
+        MVM_DEBUGFS_ADD_FILE(scan_ant_rxchain, mvm->debugfs_dir,
+                             S_IWUSR | S_IRUSR);
 #ifdef CONFIG_PM_SLEEP
         MVM_DEBUGFS_ADD_FILE(d3_sram, mvm->debugfs_dir, S_IRUSR | S_IWUSR);
         MVM_DEBUGFS_ADD_FILE(d3_test, mvm->debugfs_dir, S_IRUSR);
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-bt-coex.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-bt-coex.h
index 05c61d6f384e..4ea5e24ca92d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-bt-coex.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-bt-coex.h
@@ -82,6 +82,8 @@
  * @BT_USE_DEFAULTS:
  * @BT_SYNC_2_BT_DISABLE:
  * @BT_COEX_CORUNNING_TBL_EN:
+ *
+ * The COEX_MODE must be set for each command. Even if it is not changed.
  */
 enum iwl_bt_coex_flags {
         BT_CH_PRIMARY_EN                = BIT(0),
@@ -95,14 +97,16 @@ enum iwl_bt_coex_flags {
         BT_COEX_NW                      = 0x3 << BT_COEX_MODE_POS,
         BT_USE_DEFAULTS                 = BIT(6),
         BT_SYNC_2_BT_DISABLE            = BIT(7),
-        /*
-         * For future use - when the flags will be enlarged
-         * BT_COEX_CORUNNING_TBL_EN     = BIT(8),
-         */
+        BT_COEX_CORUNNING_TBL_EN        = BIT(8),
+        BT_COEX_MPLUT_TBL_EN            = BIT(9),
+        /* Bit 10 is reserved */
+        BT_COEX_WF_PRIO_BOOST_CHECK_EN  = BIT(11),
 };
 
 /*
  * indicates what has changed in the BT_COEX command.
+ * BT_VALID_ENABLE must be set for each command. Commands without this bit will
+ * discarded by the firmware
  */
 enum iwl_bt_coex_valid_bit_msk {
         BT_VALID_ENABLE                 = BIT(0),
@@ -121,11 +125,8 @@ enum iwl_bt_coex_valid_bit_msk {
         BT_VALID_CORUN_LUT_40           = BIT(13),
         BT_VALID_ANT_ISOLATION          = BIT(14),
         BT_VALID_ANT_ISOLATION_THRS     = BIT(15),
-        /*
-         * For future use - when the valid flags will be enlarged
-         * BT_VALID_TXTX_DELTA_FREQ_THRS        = BIT(16),
-         * BT_VALID_TXRX_MAX_FREQ_0     = BIT(17),
-         */
+        BT_VALID_TXTX_DELTA_FREQ_THRS   = BIT(16),
+        BT_VALID_TXRX_MAX_FREQ_0        = BIT(17),
 };
 
 /**
@@ -142,48 +143,88 @@ enum iwl_bt_reduced_tx_power {
         BT_REDUCED_TX_POWER_DATA        = BIT(1),
 };
 
+enum iwl_bt_coex_lut_type {
+        BT_COEX_TIGHT_LUT = 0,
+        BT_COEX_LOOSE_LUT,
+        BT_COEX_TX_DIS_LUT,
+
+        BT_COEX_MAX_LUT,
+};
+
 #define BT_COEX_LUT_SIZE (12)
+#define BT_COEX_CORUN_LUT_SIZE (32)
+#define BT_COEX_MULTI_PRIO_LUT_SIZE (2)
+#define BT_COEX_BOOST_SIZE (4)
+#define BT_REDUCED_TX_POWER_BIT BIT(7)
 
 /**
  * struct iwl_bt_coex_cmd - bt coex configuration command
  * @flags:&enum iwl_bt_coex_flags
- * @lead_time:
  * @max_kill:
- * @bt3_time_t7_value:
- * @kill_ack_msk:
- * @kill_cts_msk:
- * @bt3_prio_sample_time:
- * @bt3_timer_t2_value:
- * @bt4_reaction_time:
- * @decision_lut[12]:
  * @bt_reduced_tx_power: enum %iwl_bt_reduced_tx_power
- * @valid_bit_msk: enum %iwl_bt_coex_valid_bit_msk
- * @bt_prio_boost: values for PTA boost register
+ * @bt4_antenna_isolation:
+ * @bt4_antenna_isolation_thr:
+ * @bt4_tx_tx_delta_freq_thr:
+ * @bt4_tx_rx_max_freq0:
+ * @bt_prio_boost:
  * @wifi_tx_prio_boost: SW boost of wifi tx priority
  * @wifi_rx_prio_boost: SW boost of wifi rx priority
+ * @kill_ack_msk:
+ * @kill_cts_msk:
+ * @decision_lut:
+ * @bt4_multiprio_lut:
+ * @bt4_corun_lut20:
+ * @bt4_corun_lut40:
+ * @valid_bit_msk: enum %iwl_bt_coex_valid_bit_msk
  *
  * The structure is used for the BT_COEX command.
  */
 struct iwl_bt_coex_cmd {
-        u8 flags;
-        u8 lead_time;
+        __le32 flags;
         u8 max_kill;
-        u8 bt3_time_t7_value;
+        u8 bt_reduced_tx_power;
+        u8 reserved[2];
+
+        u8 bt4_antenna_isolation;
+        u8 bt4_antenna_isolation_thr;
+        u8 bt4_tx_tx_delta_freq_thr;
+        u8 bt4_tx_rx_max_freq0;
+
+        __le32 bt_prio_boost[BT_COEX_BOOST_SIZE];
+        __le32 wifi_tx_prio_boost;
+        __le32 wifi_rx_prio_boost;
         __le32 kill_ack_msk;
         __le32 kill_cts_msk;
-        u8 bt3_prio_sample_time;
-        u8 bt3_timer_t2_value;
-        __le16 bt4_reaction_time;
-        __le32 decision_lut[BT_COEX_LUT_SIZE];
-        u8 bt_reduced_tx_power;
-        u8 reserved;
-        __le16 valid_bit_msk;
-        __le32 bt_prio_boost;
-        u8 reserved2;
-        u8 wifi_tx_prio_boost;
-        __le16 wifi_rx_prio_boost;
+
+        __le32 decision_lut[BT_COEX_MAX_LUT][BT_COEX_LUT_SIZE];
+        __le32 bt4_multiprio_lut[BT_COEX_MULTI_PRIO_LUT_SIZE];
+        __le32 bt4_corun_lut20[BT_COEX_CORUN_LUT_SIZE];
+        __le32 bt4_corun_lut40[BT_COEX_CORUN_LUT_SIZE];
+
+        __le32 valid_bit_msk;
 } __packed; /* BT_COEX_CMD_API_S_VER_3 */
 
+/**
+ * struct iwl_bt_coex_ci_cmd - bt coex channel inhibition command
+ * @bt_primary_ci:
+ * @bt_secondary_ci:
+ * @co_run_bw_primary:
+ * @co_run_bw_secondary:
+ * @primary_ch_phy_id:
+ * @secondary_ch_phy_id:
+ *
+ * Used for BT_COEX_CI command
+ */
+struct iwl_bt_coex_ci_cmd {
+        __le64 bt_primary_ci;
+        __le64 bt_secondary_ci;
+
+        u8 co_run_bw_primary;
+        u8 co_run_bw_secondary;
+        u8 primary_ch_phy_id;
+        u8 secondary_ch_phy_id;
+} __packed; /* BT_CI_MSG_API_S_VER_1 */
+
 #define BT_MBOX(n_dw, _msg, _pos, _nbits)       \
         BT_MBOX##n_dw##_##_msg##_POS = (_pos),  \
         BT_MBOX##n_dw##_##_msg = BITS(_nbits) << BT_MBOX##n_dw##_##_msg##_POS
@@ -244,23 +285,39 @@ enum iwl_bt_mxbox_dw3 {
         ((le32_to_cpu((_notif)->mbox_msg[(_num)]) & BT_MBOX##_num##_##_field)\
         >> BT_MBOX##_num##_##_field##_POS)
 
+enum iwl_bt_activity_grading {
+        BT_OFF                  = 0,
+        BT_ON_NO_CONNECTION     = 1,
+        BT_LOW_TRAFFIC          = 2,
+        BT_HIGH_TRAFFIC         = 3,
+};
+
 /**
  * struct iwl_bt_coex_profile_notif - notification about BT coex
  * @mbox_msg: message from BT to WiFi
- * @:bt_status: 0 - off, 1 - on
- * @:bt_open_conn: number of BT connections open
- * @:bt_traffic_load: load of BT traffic
- * @:bt_agg_traffic_load: aggregated load of BT traffic
- * @:bt_ci_compliance: 0 - no CI compliance, 1 - CI compliant
+ * @msg_idx: the index of the message
+ * @bt_status: 0 - off, 1 - on
+ * @bt_open_conn: number of BT connections open
+ * @bt_traffic_load: load of BT traffic
+ * @bt_agg_traffic_load: aggregated load of BT traffic
+ * @bt_ci_compliance: 0 - no CI compliance, 1 - CI compliant
+ * @primary_ch_lut: LUT used for primary channel
+ * @secondary_ch_lut: LUT used for secondary channel
+ * @bt_activity_grading: the activity of BT enum %iwl_bt_activity_grading
  */
 struct iwl_bt_coex_profile_notif {
         __le32 mbox_msg[4];
+        __le32 msg_idx;
         u8 bt_status;
         u8 bt_open_conn;
         u8 bt_traffic_load;
         u8 bt_agg_traffic_load;
         u8 bt_ci_compliance;
         u8 reserved[3];
+
+        __le32 primary_ch_lut;
+        __le32 secondary_ch_lut;
+        __le32 bt_activity_grading;
 } __packed; /* BT_COEX_PROFILE_NTFY_API_S_VER_2 */
 
 enum iwl_bt_coex_prio_table_event {
@@ -300,20 +357,4 @@ struct iwl_bt_coex_prio_tbl_cmd {
         u8 prio_tbl[BT_COEX_PRIO_TBL_EVT_MAX];
 } __packed;
 
-enum iwl_bt_coex_env_action {
-        BT_COEX_ENV_CLOSE        = 0,
-        BT_COEX_ENV_OPEN         = 1,
-}; /* BT_COEX_PROT_ENV_ACTION_API_E_VER_1 */
-
-/**
- * struct iwl_bt_coex_prot_env_cmd - BT Protection Envelope
- * @action: enum %iwl_bt_coex_env_action
- * @type: enum %iwl_bt_coex_prio_table_event
- */
-struct iwl_bt_coex_prot_env_cmd {
-        u8 action; /* 0 = closed, 1 = open */
-        u8 type; /* 0 .. 15 */
-        u8 reserved[2];
-} __packed;
-
 #endif /* __fw_api_bt_coex_h__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
index df72fcdf8170..4e7dd8cf87dc 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
@@ -100,7 +100,12 @@ enum iwl_proto_offloads {
 
 #define IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V1     2
 #define IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V2     6
-#define IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_MAX    6
+#define IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V3L    12
+#define IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V3S    4
+#define IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_MAX    12
+
+#define IWL_PROTO_OFFLOAD_NUM_NS_CONFIG_V3L     4
+#define IWL_PROTO_OFFLOAD_NUM_NS_CONFIG_V3S     2
 
 /**
  * struct iwl_proto_offload_cmd_common - ARP/NS offload common part
@@ -155,6 +160,43 @@ struct iwl_proto_offload_cmd_v2 {
         u8 reserved2[3];
 } __packed; /* PROT_OFFLOAD_CONFIG_CMD_DB_S_VER_2 */
 
+struct iwl_ns_config {
+        struct in6_addr source_ipv6_addr;
+        struct in6_addr dest_ipv6_addr;
+        u8 target_mac_addr[ETH_ALEN];
+        __le16 reserved;
+} __packed; /* NS_OFFLOAD_CONFIG */
+
+struct iwl_targ_addr {
+        struct in6_addr addr;
+        __le32 config_num;
+} __packed; /* TARGET_IPV6_ADDRESS */
+
+/**
+ * struct iwl_proto_offload_cmd_v3_small - ARP/NS offload configuration
+ * @common: common/IPv4 configuration
+ * @target_ipv6_addr: target IPv6 addresses
+ * @ns_config: NS offload configurations
+ */
+struct iwl_proto_offload_cmd_v3_small {
+        struct iwl_proto_offload_cmd_common common;
+        __le32 num_valid_ipv6_addrs;
+        struct iwl_targ_addr targ_addrs[IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V3S];
+        struct iwl_ns_config ns_config[IWL_PROTO_OFFLOAD_NUM_NS_CONFIG_V3S];
+} __packed; /* PROT_OFFLOAD_CONFIG_CMD_DB_S_VER_3 */
+
+/**
+ * struct iwl_proto_offload_cmd_v3_large - ARP/NS offload configuration
+ * @common: common/IPv4 configuration
+ * @target_ipv6_addr: target IPv6 addresses
+ * @ns_config: NS offload configurations
+ */
+struct iwl_proto_offload_cmd_v3_large {
+        struct iwl_proto_offload_cmd_common common;
+        __le32 num_valid_ipv6_addrs;
+        struct iwl_targ_addr targ_addrs[IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V3L];
+        struct iwl_ns_config ns_config[IWL_PROTO_OFFLOAD_NUM_NS_CONFIG_V3L];
+} __packed; /* PROT_OFFLOAD_CONFIG_CMD_DB_S_VER_3 */
 
 /*
  * WOWLAN_PATTERNS
@@ -293,7 +335,7 @@ enum iwl_wowlan_wakeup_reason {
         IWL_WOWLAN_WAKEUP_BY_REM_WAKE_WAKEUP_PACKET             = BIT(12),
 }; /* WOWLAN_WAKE_UP_REASON_API_E_VER_2 */
 
-struct iwl_wowlan_status {
+struct iwl_wowlan_status_v4 {
         __le64 replay_ctr;
         __le16 pattern_number;
         __le16 non_qos_seq_ctr;
@@ -308,6 +350,29 @@ struct iwl_wowlan_status {
         u8 wake_packet[]; /* can be truncated from _length to _bufsize */
 } __packed; /* WOWLAN_STATUSES_API_S_VER_4 */
 
+struct iwl_wowlan_gtk_status {
+        u8 key_index;
+        u8 reserved[3];
+        u8 decrypt_key[16];
+        u8 tkip_mic_key[8];
+        struct iwl_wowlan_rsc_tsc_params_cmd rsc;
+} __packed;
+
+struct iwl_wowlan_status_v6 {
+        struct iwl_wowlan_gtk_status gtk;
+        __le64 replay_ctr;
+        __le16 pattern_number;
+        __le16 non_qos_seq_ctr;
+        __le16 qos_seq_ctr[8];
+        __le32 wakeup_reasons;
+        __le32 num_of_gtk_rekeys;
+        __le32 transmitted_ndps;
+        __le32 received_beacons;
+        __le32 wake_packet_length;
+        __le32 wake_packet_bufsize;
+        u8 wake_packet[]; /* can be truncated from _length to _bufsize */
+} __packed; /* WOWLAN_STATUSES_API_S_VER_6 */
+
 #define IWL_WOWLAN_TCP_MAX_PACKET_LEN           64
 #define IWL_WOWLAN_REMOTE_WAKE_MAX_PACKET_LEN   128
 #define IWL_WOWLAN_REMOTE_WAKE_MAX_TOKENS       2048
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h
index 98b1feb43d38..39c3148bdfa8 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h
@@ -170,12 +170,14 @@ struct iwl_mac_data_ap {
  * @beacon_tsf: beacon transmit time in TSF
  * @bi: beacon interval in TU
  * @bi_reciprocal: 2^32 / bi
+ * @beacon_template: beacon template ID
  */
 struct iwl_mac_data_ibss {
         __le32 beacon_time;
         __le64 beacon_tsf;
         __le32 bi;
         __le32 bi_reciprocal;
+        __le32 beacon_template;
 } __packed; /* IBSS_MAC_DATA_API_S_VER_1 */
 
 /**
@@ -372,4 +374,13 @@ static inline u32 iwl_mvm_reciprocal(u32 v)
         return 0xFFFFFFFF / v;
 }
 
+#define IWL_NONQOS_SEQ_GET      0x1
+#define IWL_NONQOS_SEQ_SET      0x2
+struct iwl_nonqos_seq_query_cmd {
+        __le32 get_set_flag;
+        __le32 mac_id_n_color;
+        __le16 value;
+        __le16 reserved;
+} __packed; /* NON_QOS_TX_COUNTER_GET_SET_API_S_VER_1 */
+
 #endif /* __fw_api_mac_h__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
index 8e7ab41079ca..5cb93ae5cd2f 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
@@ -132,6 +132,33 @@ struct iwl_powertable_cmd {
 } __packed;
 
 /**
+ * enum iwl_device_power_flags - masks for device power command flags
+ * @DEVIC_POWER_FLAGS_POWER_SAVE_ENA_MSK: '1' Allow to save power by turning off
+ *      receiver and transmitter. '0' - does not allow. This flag should be
+ *      always set to '1' unless one need to disable actual power down for debug
+ *      purposes.
+ * @DEVICE_POWER_FLAGS_CAM_MSK: '1' CAM (Continuous Active Mode) is set, meaning
+ *      that power management is disabled. '0' Power management is enabled, one
+ *      of power schemes is applied.
+*/
+enum iwl_device_power_flags {
+        DEVICE_POWER_FLAGS_POWER_SAVE_ENA_MSK   = BIT(0),
+        DEVICE_POWER_FLAGS_CAM_MSK              = BIT(13),
+};
+
+/**
+ * struct iwl_device_power_cmd - device wide power command.
+ * DEVICE_POWER_CMD = 0x77 (command, has simple generic response)
+ *
+ * @flags:      Power table command flags from DEVICE_POWER_FLAGS_*
+ */
+struct iwl_device_power_cmd {
+        /* PM_POWER_TABLE_CMD_API_S_VER_6 */
+        __le16 flags;
+        __le16 reserved;
+} __packed;
+
+/**
  * struct iwl_mac_power_cmd - New power command containing uAPSD support
  * MAC_PM_POWER_TABLE = 0xA9 (command, has simple generic response)
  * @id_and_color:       MAC contex identifier
@@ -290,7 +317,7 @@ struct iwl_beacon_filter_cmd {
 #define IWL_BF_ESCAPE_TIMER_MIN 0
 
 #define IWL_BA_ESCAPE_TIMER_DEFAULT 6
-#define IWL_BA_ESCAPE_TIMER_D3 6
+#define IWL_BA_ESCAPE_TIMER_D3 9
 #define IWL_BA_ESCAPE_TIMER_MAX 1024
 #define IWL_BA_ESCAPE_TIMER_MIN 0
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h
index fdd33bc0a594..538f1c7a5966 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h
@@ -68,6 +68,7 @@
 /*
  * These serve as indexes into
  * struct iwl_rate_info fw_rate_idx_to_plcp[IWL_RATE_COUNT];
+ * TODO: avoid overlap between legacy and HT rates
  */
 enum {
         IWL_RATE_1M_INDEX = 0,
@@ -78,18 +79,31 @@ enum {
         IWL_LAST_CCK_RATE = IWL_RATE_11M_INDEX,
         IWL_RATE_6M_INDEX,
         IWL_FIRST_OFDM_RATE = IWL_RATE_6M_INDEX,
+        IWL_RATE_MCS_0_INDEX = IWL_RATE_6M_INDEX,
+        IWL_FIRST_HT_RATE = IWL_RATE_MCS_0_INDEX,
+        IWL_FIRST_VHT_RATE = IWL_RATE_MCS_0_INDEX,
         IWL_RATE_9M_INDEX,
         IWL_RATE_12M_INDEX,
+        IWL_RATE_MCS_1_INDEX = IWL_RATE_12M_INDEX,
         IWL_RATE_18M_INDEX,
+        IWL_RATE_MCS_2_INDEX = IWL_RATE_18M_INDEX,
         IWL_RATE_24M_INDEX,
+        IWL_RATE_MCS_3_INDEX = IWL_RATE_24M_INDEX,
         IWL_RATE_36M_INDEX,
+        IWL_RATE_MCS_4_INDEX = IWL_RATE_36M_INDEX,
         IWL_RATE_48M_INDEX,
+        IWL_RATE_MCS_5_INDEX = IWL_RATE_48M_INDEX,
         IWL_RATE_54M_INDEX,
+        IWL_RATE_MCS_6_INDEX = IWL_RATE_54M_INDEX,
         IWL_LAST_NON_HT_RATE = IWL_RATE_54M_INDEX,
         IWL_RATE_60M_INDEX,
-        IWL_LAST_OFDM_RATE = IWL_RATE_60M_INDEX,
+        IWL_RATE_MCS_7_INDEX = IWL_RATE_60M_INDEX,
+        IWL_LAST_HT_RATE = IWL_RATE_MCS_7_INDEX,
+        IWL_RATE_MCS_8_INDEX,
+        IWL_RATE_MCS_9_INDEX,
+        IWL_LAST_VHT_RATE = IWL_RATE_MCS_9_INDEX,
         IWL_RATE_COUNT_LEGACY = IWL_LAST_NON_HT_RATE + 1,
-        IWL_RATE_COUNT,
+        IWL_RATE_COUNT = IWL_LAST_VHT_RATE + 1,
 };
 
 #define IWL_RATE_BIT_MSK(r) BIT(IWL_RATE_##r##M_INDEX)
@@ -108,6 +122,7 @@ enum {
         IWL_RATE_2M_PLCP  = 20,
         IWL_RATE_5M_PLCP  = 55,
         IWL_RATE_11M_PLCP = 110,
+        IWL_RATE_INVM_PLCP = -1,
 };
 
 /*
@@ -164,6 +179,8 @@ enum {
  * which is the duplicate 20 MHz MCS (bit 5 set, all others zero.)
  */
 #define RATE_HT_MCS_RATE_CODE_MSK       0x7
+#define RATE_HT_MCS_NSS_POS             3
+#define RATE_HT_MCS_NSS_MSK             (3 << RATE_HT_MCS_NSS_POS)
 
 /* Bit 10: (1) Use Green Field preamble */
 #define RATE_HT_MCS_GF_POS              10
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
index 83cb9b992ea4..c3782b48ded1 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
@@ -356,6 +356,7 @@ struct iwl_scan_complete_notif {
 /* scan offload */
 #define IWL_MAX_SCAN_CHANNELS           40
 #define IWL_SCAN_MAX_BLACKLIST_LEN      64
+#define IWL_SCAN_SHORT_BLACKLIST_LEN    16
 #define IWL_SCAN_MAX_PROFILES           11
 #define SCAN_OFFLOAD_PROBE_REQ_SIZE     512
 
@@ -368,6 +369,12 @@ struct iwl_scan_complete_notif {
 #define IWL_FULL_SCAN_MULTIPLIER 5
 #define IWL_FAST_SCHED_SCAN_ITERATIONS 3
 
+enum scan_framework_client {
+        SCAN_CLIENT_SCHED_SCAN          = BIT(0),
+        SCAN_CLIENT_NETDETECT           = BIT(1),
+        SCAN_CLIENT_ASSET_TRACKING      = BIT(2),
+};
+
 /**
  * struct iwl_scan_offload_cmd - SCAN_REQUEST_FIXED_PART_API_S_VER_6
  * @scan_flags:         see enum iwl_scan_flags
@@ -449,11 +456,12 @@ struct iwl_scan_offload_cfg {
  * iwl_scan_offload_blacklist - SCAN_OFFLOAD_BLACKLIST_S
  * @ssid:               MAC address to filter out
  * @reported_rssi:      AP rssi reported to the host
+ * @client_bitmap: clients ignore this entry  - enum scan_framework_client
  */
 struct iwl_scan_offload_blacklist {
         u8 ssid[ETH_ALEN];
         u8 reported_rssi;
-        u8 reserved;
+        u8 client_bitmap;
 } __packed;
 
 enum iwl_scan_offload_network_type {
@@ -475,6 +483,7 @@ enum iwl_scan_offload_band_selection {
  * @aut_alg:            authentication olgorithm to match - bitmap
  * @network_type:       enum iwl_scan_offload_network_type
  * @band_selection:     enum iwl_scan_offload_band_selection
+ * @client_bitmap:      clients waiting for match - enum scan_framework_client
  */
 struct iwl_scan_offload_profile {
         u8 ssid_index;
@@ -482,7 +491,8 @@ struct iwl_scan_offload_profile {
         u8 auth_alg;
         u8 network_type;
         u8 band_selection;
-        u8 reserved[3];
+        u8 client_bitmap;
+        u8 reserved[2];
 } __packed;
 
 /**
@@ -491,13 +501,18 @@ struct iwl_scan_offload_profile {
  * @profiles:           profiles to search for match
  * @blacklist_len:      length of blacklist
  * @num_profiles:       num of profiles in the list
+ * @match_notify:       clients waiting for match found notification
+ * @pass_match:         clients waiting for the results
+ * @active_clients:     active clients bitmap - enum scan_framework_client
  */
 struct iwl_scan_offload_profile_cfg {
-        struct iwl_scan_offload_blacklist blacklist[IWL_SCAN_MAX_BLACKLIST_LEN];
         struct iwl_scan_offload_profile profiles[IWL_SCAN_MAX_PROFILES];
         u8 blacklist_len;
         u8 num_profiles;
-        u8 reserved[2];
+        u8 match_notify;
+        u8 pass_match;
+        u8 active_clients;
+        u8 reserved[3];
 } __packed;
 
 /**
@@ -560,4 +575,15 @@ struct iwl_scan_offload_complete {
         u8 reserved;
 } __packed;
 
+/**
+ * iwl_sched_scan_results - SCAN_OFFLOAD_MATCH_FOUND_NTF_API_S_VER_1
+ * @ssid_bitmap:        SSIDs indexes found in this iteration
+ * @client_bitmap:      clients that are active and wait for this notification
+ */
+struct iwl_sched_scan_results {
+        __le16 ssid_bitmap;
+        u8 client_bitmap;
+        u8 reserved;
+};
+
 #endif
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
index a30691a8a85b..4aca5933a65d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
@@ -247,7 +247,7 @@ struct iwl_mvm_keyinfo {
 } __packed;
 
 /**
- * struct iwl_mvm_add_sta_cmd - Add / modify a station in the fw's station table
+ * struct iwl_mvm_add_sta_cmd_v5 - Add/modify a station in the fw's sta table.
  * ( REPLY_ADD_STA = 0x18 )
  * @add_modify: 1: modify existing, 0: add new station
  * @unicast_tx_key_id: unicast tx key id. Relevant only when unicast key sent
@@ -286,7 +286,7 @@ struct iwl_mvm_keyinfo {
  * ADD_STA sets up the table entry for one station, either creating a new
  * entry, or modifying a pre-existing one.
  */
-struct iwl_mvm_add_sta_cmd {
+struct iwl_mvm_add_sta_cmd_v5 {
         u8 add_modify;
         u8 unicast_tx_key_id;
         u8 multicast_tx_key_id;
@@ -313,6 +313,57 @@ struct iwl_mvm_add_sta_cmd {
 } __packed; /* ADD_STA_CMD_API_S_VER_5 */
 
 /**
+ * struct iwl_mvm_add_sta_cmd_v6 - Add / modify a station
+ * VER_6 of this command is quite similar to VER_5 except
+ * exclusion of all fields related to the security key installation.
+ */
+struct iwl_mvm_add_sta_cmd_v6 {
+        u8 add_modify;
+        u8 reserved1;
+        __le16 tid_disable_tx;
+        __le32 mac_id_n_color;
+        u8 addr[ETH_ALEN];      /* _STA_ID_MODIFY_INFO_API_S_VER_1 */
+        __le16 reserved2;
+        u8 sta_id;
+        u8 modify_mask;
+        __le16 reserved3;
+        __le32 station_flags;
+        __le32 station_flags_msk;
+        u8 add_immediate_ba_tid;
+        u8 remove_immediate_ba_tid;
+        __le16 add_immediate_ba_ssn;
+        __le16 sleep_tx_count;
+        __le16 sleep_state_flags;
+        __le16 assoc_id;
+        __le16 beamform_flags;
+        __le32 tfd_queue_msk;
+} __packed; /* ADD_STA_CMD_API_S_VER_6 */
+
+/**
+ * struct iwl_mvm_add_sta_key_cmd - add/modify sta key
+ * ( REPLY_ADD_STA_KEY = 0x17 )
+ * @sta_id: index of station in uCode's station table
+ * @key_offset: key offset in key storage
+ * @key_flags: type %iwl_sta_key_flag
+ * @key: key material data
+ * @key2: key material data
+ * @rx_secur_seq_cnt: RX security sequence counter for the key
+ * @tkip_rx_tsc_byte2: TSC[2] for key mix ph1 detection
+ * @tkip_rx_ttak: 10-byte unicast TKIP TTAK for Rx
+ */
+struct iwl_mvm_add_sta_key_cmd {
+        u8 sta_id;
+        u8 key_offset;
+        __le16 key_flags;
+        u8 key[16];
+        u8 key2[16];
+        u8 rx_secur_seq_cnt[16];
+        u8 tkip_rx_tsc_byte2;
+        u8 reserved;
+        __le16 tkip_rx_ttak[5];
+} __packed; /* ADD_MODIFY_STA_KEY_API_S_VER_1 */
+
+/**
  * enum iwl_mvm_add_sta_rsp_status - status in the response to ADD_STA command
  * @ADD_STA_SUCCESS: operation was executed successfully
  * @ADD_STA_STATIONS_OVERLOAD: no room left in the fw's station table
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api.h b/drivers/net/wireless/iwlwifi/mvm/fw-api.h
index 66264cc5a016..bad5a552dd8d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api.h
@@ -72,17 +72,17 @@
 #include "fw-api-d3.h"
 #include "fw-api-bt-coex.h"
 
-/* queue and FIFO numbers by usage */
+/* maximal number of Tx queues in any platform */
+#define IWL_MVM_MAX_QUEUES      20
+
+/* Tx queue numbers */
 enum {
         IWL_MVM_OFFCHANNEL_QUEUE = 8,
         IWL_MVM_CMD_QUEUE = 9,
-        IWL_MVM_AUX_QUEUE = 15,
-        IWL_MVM_FIRST_AGG_QUEUE = 16,
-        IWL_MVM_NUM_QUEUES = 20,
-        IWL_MVM_LAST_AGG_QUEUE = IWL_MVM_NUM_QUEUES - 1,
-        IWL_MVM_CMD_FIFO = 7
 };
 
+#define IWL_MVM_CMD_FIFO        7
+
 #define IWL_MVM_STATION_COUNT   16
 
 /* commands */
@@ -97,6 +97,7 @@ enum {
         DBG_CFG = 0x9,
 
         /* station table */
+        ADD_STA_KEY = 0x17,
         ADD_STA = 0x18,
         REMOVE_STA = 0x19,
 
@@ -114,6 +115,7 @@ enum {
         TIME_EVENT_NOTIFICATION = 0x2a,
         BINDING_CONTEXT_CMD = 0x2b,
         TIME_QUOTA_CMD = 0x2c,
+        NON_QOS_TX_COUNTER_CMD = 0x2d,
 
         LQ_CMD = 0x4e,
 
@@ -130,6 +132,7 @@ enum {
         SCAN_OFFLOAD_COMPLETE = 0x6D,
         SCAN_OFFLOAD_UPDATE_PROFILES_CMD = 0x6E,
         SCAN_OFFLOAD_CONFIG_CMD = 0x6f,
+        MATCH_FOUND_NOTIFICATION = 0xd9,
 
         /* Phy */
         PHY_CONFIGURATION_CMD = 0x6a,
@@ -178,6 +181,7 @@ enum {
         BT_COEX_PRIO_TABLE = 0xcc,
         BT_COEX_PROT_ENV = 0xcd,
         BT_PROFILE_NOTIFICATION = 0xce,
+        BT_COEX_CI = 0x5d,
 
         REPLY_BEACON_FILTERING_CMD = 0xd2,
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw.c b/drivers/net/wireless/iwlwifi/mvm/fw.c
index c76299a3a1e0..83fc5ca04433 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw.c
+++ b/drivers/net/wireless/iwlwifi/mvm/fw.c
@@ -199,7 +199,7 @@ static int iwl_mvm_load_ucode_wait_alive(struct iwl_mvm *mvm,
          */
 
         for (i = 0; i < IWL_MAX_HW_QUEUES; i++) {
-                if (i < IWL_MVM_FIRST_AGG_QUEUE && i != IWL_MVM_CMD_QUEUE)
+                if (i < mvm->first_agg_queue && i != IWL_MVM_CMD_QUEUE)
                         mvm->queue_to_mac80211[i] = i;
                 else
                         mvm->queue_to_mac80211[i] = IWL_INVALID_MAC80211_QUEUE;
@@ -243,7 +243,7 @@ int iwl_run_init_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm)
 
         lockdep_assert_held(&mvm->mutex);
 
-        if (mvm->init_ucode_run)
+        if (mvm->init_ucode_complete)
                 return 0;
 
         iwl_init_notification_wait(&mvm->notif_wait,
@@ -264,6 +264,7 @@ int iwl_run_init_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm)
         if (ret)
                 goto error;
 
+        /* Read the NVM only at driver load time, no need to do this twice */
         if (read_nvm) {
                 /* Read nvm */
                 ret = iwl_nvm_init(mvm);
@@ -273,6 +274,10 @@ int iwl_run_init_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm)
                 }
         }
 
+        /* In case we read the NVM from external file, load it to the NIC */
+        if (iwlwifi_mod_params.nvm_file)
+                iwl_mvm_load_nvm_to_nic(mvm);
+
         ret = iwl_nvm_check_version(mvm->nvm_data, mvm->trans);
         WARN_ON(ret);
 
@@ -310,7 +315,7 @@ int iwl_run_init_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm)
         ret = iwl_wait_notification(&mvm->notif_wait, &calib_wait,
                         MVM_UCODE_CALIB_TIMEOUT);
         if (!ret)
-                mvm->init_ucode_run = true;
+                mvm->init_ucode_complete = true;
         goto out;
 
 error:
@@ -353,8 +358,12 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
         if (ret)
                 return ret;
 
-        /* If we were in RFKILL during module loading, load init ucode now */
-        if (!mvm->init_ucode_run) {
+        /*
+         * If we haven't completed the run of the init ucode during
+         * module loading, load init ucode now
+         * (for example, if we were in RFKILL)
+         */
+        if (!mvm->init_ucode_complete) {
                 ret = iwl_run_init_mvm_ucode(mvm, false);
                 if (ret && !iwlmvm_mod_params.init_dbg) {
                         IWL_ERR(mvm, "Failed to run INIT ucode: %d\n", ret);
@@ -424,6 +433,10 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
                         goto error;
         }
 
+        ret = iwl_mvm_power_update_device_mode(mvm);
+        if (ret)
+                goto error;
+
         IWL_DEBUG_INFO(mvm, "RT uCode started.\n");
         return 0;
  error:
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
index 5fe23a5ea9b6..ab5a7ac90dcd 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
@@ -80,7 +80,7 @@ struct iwl_mvm_mac_iface_iterator_data {
         struct ieee80211_vif *vif;
         unsigned long available_mac_ids[BITS_TO_LONGS(NUM_MAC_INDEX_DRIVER)];
         unsigned long available_tsf_ids[BITS_TO_LONGS(NUM_TSF_IDS)];
-        unsigned long used_hw_queues[BITS_TO_LONGS(IWL_MVM_FIRST_AGG_QUEUE)];
+        unsigned long used_hw_queues[BITS_TO_LONGS(IWL_MVM_MAX_QUEUES)];
         enum iwl_tsf_id preferred_tsf;
         bool found_vif;
 };
@@ -218,7 +218,7 @@ static int iwl_mvm_mac_ctxt_allocate_resources(struct iwl_mvm *mvm,
                 .preferred_tsf = NUM_TSF_IDS,
                 .used_hw_queues = {
                         BIT(IWL_MVM_OFFCHANNEL_QUEUE) |
-                        BIT(IWL_MVM_AUX_QUEUE) |
+                        BIT(mvm->aux_queue) |
                         BIT(IWL_MVM_CMD_QUEUE)
                 },
                 .found_vif = false,
@@ -242,9 +242,17 @@ static int iwl_mvm_mac_ctxt_allocate_resources(struct iwl_mvm *mvm,
          * that we should share it with another interface.
          */
 
-        /* Currently, MAC ID 0 should be used only for the managed vif */
-        if (vif->type != NL80211_IFTYPE_STATION || vif->p2p)
+        /* Currently, MAC ID 0 should be used only for the managed/IBSS vif */
+        switch (vif->type) {
+        case NL80211_IFTYPE_ADHOC:
+                break;
+        case NL80211_IFTYPE_STATION:
+                if (!vif->p2p)
+                        break;
+                /* fall through */
+        default:
                 __clear_bit(0, data.available_mac_ids);
+        }
 
         ieee80211_iterate_active_interfaces_atomic(
                 mvm->hw, IEEE80211_IFACE_ITER_RESUME_ALL,
@@ -302,9 +310,9 @@ static int iwl_mvm_mac_ctxt_allocate_resources(struct iwl_mvm *mvm,
         /* Find available queues, and allocate them to the ACs */
         for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
                 u8 queue = find_first_zero_bit(data.used_hw_queues,
-                                               IWL_MVM_FIRST_AGG_QUEUE);
+                                               mvm->first_agg_queue);
 
-                if (queue >= IWL_MVM_FIRST_AGG_QUEUE) {
+                if (queue >= mvm->first_agg_queue) {
                         IWL_ERR(mvm, "Failed to allocate queue\n");
                         ret = -EIO;
                         goto exit_fail;
@@ -317,9 +325,9 @@ static int iwl_mvm_mac_ctxt_allocate_resources(struct iwl_mvm *mvm,
         /* Allocate the CAB queue for softAP and GO interfaces */
         if (vif->type == NL80211_IFTYPE_AP) {
                 u8 queue = find_first_zero_bit(data.used_hw_queues,
-                                               IWL_MVM_FIRST_AGG_QUEUE);
+                                               mvm->first_agg_queue);
 
-                if (queue >= IWL_MVM_FIRST_AGG_QUEUE) {
+                if (queue >= mvm->first_agg_queue) {
                         IWL_ERR(mvm, "Failed to allocate cab queue\n");
                         ret = -EIO;
                         goto exit_fail;
@@ -559,8 +567,12 @@ static void iwl_mvm_mac_ctxt_cmd_common(struct iwl_mvm *mvm,
                 cmd->qos_flags |= cpu_to_le32(MAC_QOS_FLG_UPDATE_EDCA);
 
         /* Don't use cts to self as the fw doesn't support it currently. */
-        if (vif->bss_conf.use_cts_prot)
+        if (vif->bss_conf.use_cts_prot) {
                 cmd->protection_flags |= cpu_to_le32(MAC_PROT_FLG_TGG_PROTECT);
+                if (IWL_UCODE_API(mvm->fw->ucode_ver) >= 8)
+                        cmd->protection_flags |=
+                                cpu_to_le32(MAC_PROT_FLG_SELF_CTS_EN);
+        }
 
         /*
          * I think that we should enable these 2 flags regardless the HT PROT
@@ -712,6 +724,31 @@ static int iwl_mvm_mac_ctxt_cmd_listener(struct iwl_mvm *mvm,
         return iwl_mvm_mac_ctxt_send_cmd(mvm, &cmd);
 }
 
+static int iwl_mvm_mac_ctxt_cmd_ibss(struct iwl_mvm *mvm,
+                                     struct ieee80211_vif *vif,
+                                     u32 action)
+{
+        struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+        struct iwl_mac_ctx_cmd cmd = {};
+
+        WARN_ON(vif->type != NL80211_IFTYPE_ADHOC);
+
+        iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, action);
+
+        cmd.filter_flags = cpu_to_le32(MAC_FILTER_IN_BEACON |
+                                       MAC_FILTER_IN_PROBE_REQUEST);
+
+        /* cmd.ibss.beacon_time/cmd.ibss.beacon_tsf are curently ignored */
+        cmd.ibss.bi = cpu_to_le32(vif->bss_conf.beacon_int);
+        cmd.ibss.bi_reciprocal =
+                cpu_to_le32(iwl_mvm_reciprocal(vif->bss_conf.beacon_int));
+
+        /* TODO: Assumes that the beacon id == mac context id */
+        cmd.ibss.beacon_template = cpu_to_le32(mvmvif->id);
+
+        return iwl_mvm_mac_ctxt_send_cmd(mvm, &cmd);
+}
+
 struct iwl_mvm_go_iterator_data {
         bool go_active;
 };
@@ -721,7 +758,8 @@ static void iwl_mvm_go_iterator(void *_data, u8 *mac, struct ieee80211_vif *vif)
         struct iwl_mvm_go_iterator_data *data = _data;
         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 
-        if (vif->type == NL80211_IFTYPE_AP && vif->p2p && mvmvif->ap_active)
+        if (vif->type == NL80211_IFTYPE_AP && vif->p2p &&
+            mvmvif->ap_ibss_active)
                 data->go_active = true;
 }
 
@@ -833,9 +871,10 @@ static int iwl_mvm_mac_ctxt_send_beacon(struct iwl_mvm *mvm,
                 cpu_to_le32(iwl_mvm_mac80211_idx_to_hwrate(rate));
 
         /* Set up TX beacon command fields */
-        iwl_mvm_mac_ctxt_set_tim(mvm, &beacon_cmd,
-                                 beacon->data,
-                                 beacon_skb_len);
+        if (vif->type == NL80211_IFTYPE_AP)
+                iwl_mvm_mac_ctxt_set_tim(mvm, &beacon_cmd,
+                                         beacon->data,
+                                         beacon_skb_len);
 
         /* Submit command */
         cmd.len[0] = sizeof(beacon_cmd);
@@ -848,14 +887,15 @@ static int iwl_mvm_mac_ctxt_send_beacon(struct iwl_mvm *mvm,
         return iwl_mvm_send_cmd(mvm, &cmd);
 }
 
-/* The beacon template for the AP/GO context has changed and needs update */
+/* The beacon template for the AP/GO/IBSS has changed and needs update */
 int iwl_mvm_mac_ctxt_beacon_changed(struct iwl_mvm *mvm,
                                     struct ieee80211_vif *vif)
 {
         struct sk_buff *beacon;
         int ret;
 
-        WARN_ON(vif->type != NL80211_IFTYPE_AP);
+        WARN_ON(vif->type != NL80211_IFTYPE_AP &&
+                vif->type != NL80211_IFTYPE_ADHOC);
 
         beacon = ieee80211_beacon_get(mvm->hw, vif);
         if (!beacon)
@@ -1018,6 +1058,8 @@ static int iwl_mvm_mac_ctx_send(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
                 return iwl_mvm_mac_ctxt_cmd_listener(mvm, vif, action);
         case NL80211_IFTYPE_P2P_DEVICE:
                 return iwl_mvm_mac_ctxt_cmd_p2p_device(mvm, vif, action);
+        case NL80211_IFTYPE_ADHOC:
+                return iwl_mvm_mac_ctxt_cmd_ibss(mvm, vif, action);
         default:
                 break;
         }
@@ -1038,6 +1080,9 @@ int iwl_mvm_mac_ctxt_add(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
         if (ret)
                 return ret;
 
+        /* will only do anything at resume from D3 time */
+        iwl_mvm_set_last_nonqos_seq(mvm, vif);
+
         mvmvif->uploaded = true;
         return 0;
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
index 9833cdf6177c..f40685c3764e 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
@@ -77,6 +77,7 @@
 #include "iwl-eeprom-parse.h"
 #include "fw-api-scan.h"
 #include "iwl-phy-db.h"
+#include "testmode.h"
 
 static const struct ieee80211_iface_limit iwl_mvm_limits[] = {
         {
@@ -138,6 +139,14 @@ static void iwl_mvm_reset_phy_ctxts(struct iwl_mvm *mvm)
         }
 }
 
+static int iwl_mvm_max_scan_ie_len(struct iwl_mvm *mvm)
+{
+        /* we create the 802.11 header and SSID element */
+        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_NO_BASIC_SSID)
+                return mvm->fw->ucode_capa.max_probe_length - 24 - 2;
+        return mvm->fw->ucode_capa.max_probe_length - 24 - 34;
+}
+
 int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 {
         struct ieee80211_hw *hw = mvm->hw;
@@ -158,7 +167,7 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
                     IEEE80211_HW_SUPPORTS_STATIC_SMPS |
                     IEEE80211_HW_SUPPORTS_UAPSD;
 
-        hw->queues = IWL_MVM_FIRST_AGG_QUEUE;
+        hw->queues = mvm->first_agg_queue;
         hw->offchannel_tx_hw_queue = IWL_MVM_OFFCHANNEL_QUEUE;
         hw->rate_control_algorithm = "iwl-mvm-rs";
 
@@ -181,6 +190,10 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
                 BIT(NL80211_IFTYPE_P2P_GO) |
                 BIT(NL80211_IFTYPE_P2P_DEVICE);
 
+        /* IBSS has bugs in older versions */
+        if (IWL_UCODE_API(mvm->fw->ucode_ver) >= 8)
+                hw->wiphy->interface_modes |= BIT(NL80211_IFTYPE_ADHOC);
+
         hw->wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY |
                             WIPHY_FLAG_DISABLE_BEACON_HINTS |
                             WIPHY_FLAG_IBSS_RSN;
@@ -212,9 +225,8 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 
         iwl_mvm_reset_phy_ctxts(mvm);
 
-        /* we create the 802.11 header and a max-length SSID element */
-        hw->wiphy->max_scan_ie_len =
-                mvm->fw->ucode_capa.max_probe_length - 24 - 34;
+        hw->wiphy->max_scan_ie_len = iwl_mvm_max_scan_ie_len(mvm);
+
         hw->wiphy->max_scan_ssids = PROBE_OPTION_MAX;
 
         if (mvm->nvm_data->bands[IEEE80211_BAND_2GHZ].n_channels)
@@ -231,6 +243,15 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
         else
                 hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
+        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_SCHED_SCAN) {
+                hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_SCHED_SCAN;
+                hw->wiphy->max_sched_scan_ssids = PROBE_OPTION_MAX;
+                hw->wiphy->max_match_sets = IWL_SCAN_MAX_PROFILES;
+                /* we create the 802.11 header and zero length SSID IE. */
+                hw->wiphy->max_sched_scan_ie_len =
+                                        SCAN_OFFLOAD_PROBE_REQ_SIZE - 24 - 2;
+        }
+
         hw->wiphy->features |= NL80211_FEATURE_P2P_GO_CTWIN |
                                NL80211_FEATURE_P2P_GO_OPPPS;
 
@@ -548,7 +569,8 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
          * In short: there's not much we can do at this point, other than
          * allocating resources :)
          */
-        if (vif->type == NL80211_IFTYPE_AP) {
+        if (vif->type == NL80211_IFTYPE_AP ||
+            vif->type == NL80211_IFTYPE_ADHOC) {
                 u32 qmask = iwl_mvm_mac_get_queues_mask(mvm, vif);
                 ret = iwl_mvm_allocate_int_sta(mvm, &mvmvif->bcast_sta,
                                                qmask);
@@ -698,7 +720,14 @@ static void iwl_mvm_mac_remove_interface(struct ieee80211_hw *hw,
          * For AP/GO interface, the tear down of the resources allocated to the
          * interface is be handled as part of the stop_ap flow.
          */
-        if (vif->type == NL80211_IFTYPE_AP) {
+        if (vif->type == NL80211_IFTYPE_AP ||
+            vif->type == NL80211_IFTYPE_ADHOC) {
+#ifdef CONFIG_NL80211_TESTMODE
+                if (vif == mvm->noa_vif) {
+                        mvm->noa_vif = NULL;
+                        mvm->noa_duration = 0;
+                }
+#endif
                 iwl_mvm_dealloc_int_sta(mvm, &mvmvif->bcast_sta);
                 goto out_release;
         }
@@ -796,6 +825,27 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
                                 return;
                         }
                         iwl_mvm_configure_mcast_filter(mvm, vif);
+
+                        if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART,
+                                     &mvm->status)) {
+                                /*
+                                 * If we're restarting then the firmware will
+                                 * obviously have lost synchronisation with
+                                 * the AP. It will attempt to synchronise by
+                                 * itself, but we can make it more reliable by
+                                 * scheduling a session protection time event.
+                                 *
+                                 * The firmware needs to receive a beacon to
+                                 * catch up with synchronisation, use 110% of
+                                 * the beacon interval.
+                                 *
+                                 * Set a large maximum delay to allow for more
+                                 * than a single interface.
+                                 */
+                                u32 dur = (11 * vif->bss_conf.beacon_int) / 10;
+                                iwl_mvm_protect_session(mvm, vif, dur, dur,
+                                                        5 * dur);
+                        }
                 } else if (mvmvif->ap_sta_id != IWL_MVM_STATION_COUNT) {
                         /* remove AP station now that the MAC is unassoc */
                         ret = iwl_mvm_rm_sta_id(mvm, vif, mvmvif->ap_sta_id);
@@ -819,7 +869,7 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
                         if (ret)
                                 IWL_ERR(mvm, "failed to update power mode\n");
                 }
-                iwl_mvm_bt_coex_vif_assoc(mvm, vif);
+                iwl_mvm_bt_coex_vif_change(mvm);
         } else if (changes & BSS_CHANGED_BEACON_INFO) {
                 /*
                  * We received a beacon _after_ association so
@@ -848,7 +898,8 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
         }
 }
 
-static int iwl_mvm_start_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+static int iwl_mvm_start_ap_ibss(struct ieee80211_hw *hw,
+                                 struct ieee80211_vif *vif)
 {
         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
@@ -871,7 +922,7 @@ static int iwl_mvm_start_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
         if (ret)
                 goto out_remove;
 
-        mvmvif->ap_active = true;
+        mvmvif->ap_ibss_active = true;
 
         /* Send the bcast station. At this stage the TBTT and DTIM time events
          * are added and applied to the scheduler */
@@ -883,10 +934,12 @@ static int iwl_mvm_start_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
         if (ret)
                 goto out_rm_bcast;
 
-        /* Need to update the P2P Device MAC */
+        /* Need to update the P2P Device MAC (only GO, IBSS is single vif) */
         if (vif->p2p && mvm->p2p_device_vif)
                 iwl_mvm_mac_ctxt_changed(mvm, mvm->p2p_device_vif);
 
+        iwl_mvm_bt_coex_vif_change(mvm);
+
         mutex_unlock(&mvm->mutex);
         return 0;
 
@@ -901,7 +954,8 @@ out_unlock:
         return ret;
 }
 
-static void iwl_mvm_stop_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+static void iwl_mvm_stop_ap_ibss(struct ieee80211_hw *hw,
+                                 struct ieee80211_vif *vif)
 {
         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
@@ -910,9 +964,11 @@ static void iwl_mvm_stop_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 
         mutex_lock(&mvm->mutex);
 
-        mvmvif->ap_active = false;
+        mvmvif->ap_ibss_active = false;
+
+        iwl_mvm_bt_coex_vif_change(mvm);
 
-        /* Need to update the P2P Device MAC */
+        /* Need to update the P2P Device MAC (only GO, IBSS is single vif) */
         if (vif->p2p && mvm->p2p_device_vif)
                 iwl_mvm_mac_ctxt_changed(mvm, mvm->p2p_device_vif);
 
@@ -924,10 +980,11 @@ static void iwl_mvm_stop_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
         mutex_unlock(&mvm->mutex);
 }
 
-static void iwl_mvm_bss_info_changed_ap(struct iwl_mvm *mvm,
-                                        struct ieee80211_vif *vif,
-                                        struct ieee80211_bss_conf *bss_conf,
-                                        u32 changes)
+static void
+iwl_mvm_bss_info_changed_ap_ibss(struct iwl_mvm *mvm,
+                                 struct ieee80211_vif *vif,
+                                 struct ieee80211_bss_conf *bss_conf,
+                                 u32 changes)
 {
         /* Need to send a new beacon template to the FW */
         if (changes & BSS_CHANGED_BEACON) {
@@ -950,7 +1007,8 @@ static void iwl_mvm_bss_info_changed(struct ieee80211_hw *hw,
                 iwl_mvm_bss_info_changed_station(mvm, vif, bss_conf, changes);
                 break;
         case NL80211_IFTYPE_AP:
-                iwl_mvm_bss_info_changed_ap(mvm, vif, bss_conf, changes);
+        case NL80211_IFTYPE_ADHOC:
+                iwl_mvm_bss_info_changed_ap_ibss(mvm, vif, bss_conf, changes);
                 break;
         default:
                 /* shouldn't happen */
@@ -1163,7 +1221,54 @@ static void iwl_mvm_mac_mgd_prepare_tx(struct ieee80211_hw *hw,
 
         mutex_lock(&mvm->mutex);
         /* Try really hard to protect the session and hear a beacon */
-        iwl_mvm_protect_session(mvm, vif, duration, min_duration);
+        iwl_mvm_protect_session(mvm, vif, duration, min_duration, 500);
+        mutex_unlock(&mvm->mutex);
+}
+
+static int iwl_mvm_mac_sched_scan_start(struct ieee80211_hw *hw,
+                                        struct ieee80211_vif *vif,
+                                        struct cfg80211_sched_scan_request *req,
+                                        struct ieee80211_sched_scan_ies *ies)
+{
+        struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+        int ret;
+
+        mutex_lock(&mvm->mutex);
+
+        if (mvm->scan_status != IWL_MVM_SCAN_NONE) {
+                IWL_DEBUG_SCAN(mvm,
+                               "SCHED SCAN request during internal scan - abort\n");
+                ret = -EBUSY;
+                goto out;
+        }
+
+        mvm->scan_status = IWL_MVM_SCAN_SCHED;
+
+        ret = iwl_mvm_config_sched_scan(mvm, vif, req, ies);
+        if (ret)
+                goto err;
+
+        ret = iwl_mvm_config_sched_scan_profiles(mvm, req);
+        if (ret)
+                goto err;
+
+        ret = iwl_mvm_sched_scan_start(mvm, req);
+        if (!ret)
+                goto out;
+err:
+        mvm->scan_status = IWL_MVM_SCAN_NONE;
+out:
+        mutex_unlock(&mvm->mutex);
+        return ret;
+}
+
+static void iwl_mvm_mac_sched_scan_stop(struct ieee80211_hw *hw,
+                                        struct ieee80211_vif *vif)
+{
+        struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+
+        mutex_lock(&mvm->mutex);
+        iwl_mvm_sched_scan_stop(mvm);
         mutex_unlock(&mvm->mutex);
 }
 
@@ -1207,8 +1312,13 @@ static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
 
         switch (cmd) {
         case SET_KEY:
-                if (vif->type == NL80211_IFTYPE_AP && !sta) {
-                        /* GTK on AP interface is a TX-only key, return 0 */
+                if ((vif->type == NL80211_IFTYPE_ADHOC ||
+                     vif->type == NL80211_IFTYPE_AP) && !sta) {
+                        /*
+                         * GTK on AP interface is a TX-only key, return 0;
+                         * on IBSS they're per-station and because we're lazy
+                         * we don't support them for RX, so do the same.
+                         */
                         ret = 0;
                         key->hw_key_idx = STA_KEY_IDX_INVALID;
                         break;
@@ -1252,6 +1362,9 @@ static void iwl_mvm_mac_update_tkip_key(struct ieee80211_hw *hw,
 {
         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 
+        if (keyconf->hw_key_idx == STA_KEY_IDX_INVALID)
+                return;
+
         iwl_mvm_update_tkip_key(mvm, vif, keyconf, sta, iv32, phase1key);
 }
 
@@ -1445,6 +1558,7 @@ static void iwl_mvm_change_chanctx(struct ieee80211_hw *hw,
         iwl_mvm_phy_ctxt_changed(mvm, phy_ctxt, &ctx->def,
                                  ctx->rx_chains_static,
                                  ctx->rx_chains_dynamic);
+        iwl_mvm_bt_coex_vif_change(mvm);
         mutex_unlock(&mvm->mutex);
 }
 
@@ -1464,14 +1578,14 @@ static int iwl_mvm_assign_vif_chanctx(struct ieee80211_hw *hw,
 
         switch (vif->type) {
         case NL80211_IFTYPE_AP:
+        case NL80211_IFTYPE_ADHOC:
                 /*
                  * The AP binding flow is handled as part of the start_ap flow
-                 * (in bss_info_changed).
+                 * (in bss_info_changed), similarly for IBSS.
                  */
                 ret = 0;
                 goto out_unlock;
         case NL80211_IFTYPE_STATION:
-        case NL80211_IFTYPE_ADHOC:
         case NL80211_IFTYPE_MONITOR:
                 break;
         default:
@@ -1517,10 +1631,10 @@ static void iwl_mvm_unassign_vif_chanctx(struct ieee80211_hw *hw,
 
         iwl_mvm_remove_time_event(mvm, mvmvif, &mvmvif->time_event_data);
 
-        if (vif->type == NL80211_IFTYPE_AP)
-                goto out_unlock;
-
         switch (vif->type) {
+        case NL80211_IFTYPE_AP:
+        case NL80211_IFTYPE_ADHOC:
+                goto out_unlock;
         case NL80211_IFTYPE_MONITOR:
                 mvmvif->monitor_active = false;
                 iwl_mvm_update_quotas(mvm, NULL);
@@ -1550,14 +1664,72 @@ static int iwl_mvm_set_tim(struct ieee80211_hw *hw,
         return iwl_mvm_mac_ctxt_beacon_changed(mvm, mvm_sta->vif);
 }
 
-static void iwl_mvm_mac_rssi_callback(struct ieee80211_hw *hw,
+#ifdef CONFIG_NL80211_TESTMODE
+static const struct nla_policy iwl_mvm_tm_policy[IWL_MVM_TM_ATTR_MAX + 1] = {
+        [IWL_MVM_TM_ATTR_CMD] = { .type = NLA_U32 },
+        [IWL_MVM_TM_ATTR_NOA_DURATION] = { .type = NLA_U32 },
+        [IWL_MVM_TM_ATTR_BEACON_FILTER_STATE] = { .type = NLA_U32 },
+};
+
+static int __iwl_mvm_mac_testmode_cmd(struct iwl_mvm *mvm,
                                       struct ieee80211_vif *vif,
-                                      enum ieee80211_rssi_event rssi_event)
+                                      void *data, int len)
+{
+        struct nlattr *tb[IWL_MVM_TM_ATTR_MAX + 1];
+        int err;
+        u32 noa_duration;
+
+        err = nla_parse(tb, IWL_MVM_TM_ATTR_MAX, data, len, iwl_mvm_tm_policy);
+        if (err)
+                return err;
+
+        if (!tb[IWL_MVM_TM_ATTR_CMD])
+                return -EINVAL;
+
+        switch (nla_get_u32(tb[IWL_MVM_TM_ATTR_CMD])) {
+        case IWL_MVM_TM_CMD_SET_NOA:
+                if (!vif || vif->type != NL80211_IFTYPE_AP || !vif->p2p ||
+                    !vif->bss_conf.enable_beacon ||
+                    !tb[IWL_MVM_TM_ATTR_NOA_DURATION])
+                        return -EINVAL;
+
+                noa_duration = nla_get_u32(tb[IWL_MVM_TM_ATTR_NOA_DURATION]);
+                if (noa_duration >= vif->bss_conf.beacon_int)
+                        return -EINVAL;
+
+                mvm->noa_duration = noa_duration;
+                mvm->noa_vif = vif;
+
+                return iwl_mvm_update_quotas(mvm, NULL);
+        case IWL_MVM_TM_CMD_SET_BEACON_FILTER:
+                /* must be associated client vif - ignore authorized */
+                if (!vif || vif->type != NL80211_IFTYPE_STATION ||
+                    !vif->bss_conf.assoc || !vif->bss_conf.dtim_period ||
+                    !tb[IWL_MVM_TM_ATTR_BEACON_FILTER_STATE])
+                        return -EINVAL;
+
+                if (nla_get_u32(tb[IWL_MVM_TM_ATTR_BEACON_FILTER_STATE]))
+                        return iwl_mvm_enable_beacon_filter(mvm, vif);
+                return iwl_mvm_disable_beacon_filter(mvm, vif);
+        }
+
+        return -EOPNOTSUPP;
+}
+
+static int iwl_mvm_mac_testmode_cmd(struct ieee80211_hw *hw,
+                                    struct ieee80211_vif *vif,
+                                    void *data, int len)
 {
         struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+        int err;
 
-        iwl_mvm_bt_rssi_event(mvm, vif, rssi_event);
+        mutex_lock(&mvm->mutex);
+        err = __iwl_mvm_mac_testmode_cmd(mvm, vif, data, len);
+        mutex_unlock(&mvm->mutex);
+
+        return err;
 }
+#endif
 
 struct ieee80211_ops iwl_mvm_hw_ops = {
         .tx = iwl_mvm_mac_tx,
@@ -1578,23 +1750,27 @@ struct ieee80211_ops iwl_mvm_hw_ops = {
         .set_rts_threshold = iwl_mvm_mac_set_rts_threshold,
         .conf_tx = iwl_mvm_mac_conf_tx,
         .mgd_prepare_tx = iwl_mvm_mac_mgd_prepare_tx,
+        .sched_scan_start = iwl_mvm_mac_sched_scan_start,
+        .sched_scan_stop = iwl_mvm_mac_sched_scan_stop,
         .set_key = iwl_mvm_mac_set_key,
         .update_tkip_key = iwl_mvm_mac_update_tkip_key,
         .remain_on_channel = iwl_mvm_roc,
         .cancel_remain_on_channel = iwl_mvm_cancel_roc,
-        .rssi_callback = iwl_mvm_mac_rssi_callback,
-
         .add_chanctx = iwl_mvm_add_chanctx,
         .remove_chanctx = iwl_mvm_remove_chanctx,
         .change_chanctx = iwl_mvm_change_chanctx,
         .assign_vif_chanctx = iwl_mvm_assign_vif_chanctx,
         .unassign_vif_chanctx = iwl_mvm_unassign_vif_chanctx,
 
-        .start_ap = iwl_mvm_start_ap,
-        .stop_ap = iwl_mvm_stop_ap,
+        .start_ap = iwl_mvm_start_ap_ibss,
+        .stop_ap = iwl_mvm_stop_ap_ibss,
+        .join_ibss = iwl_mvm_start_ap_ibss,
+        .leave_ibss = iwl_mvm_stop_ap_ibss,
 
         .set_tim = iwl_mvm_set_tim,
 
+        CFG80211_TESTMODE_CMD(iwl_mvm_mac_testmode_cmd)
+
 #ifdef CONFIG_PM_SLEEP
         /* look at d3.c */
         .suspend = iwl_mvm_suspend,
diff --git a/drivers/net/wireless/iwlwifi/mvm/mvm.h b/drivers/net/wireless/iwlwifi/mvm/mvm.h
index b0389279cc1e..6235cb729f5c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/iwlwifi/mvm/mvm.h
@@ -162,6 +162,7 @@ enum iwl_power_scheme {
 struct iwl_mvm_power_ops {
         int (*power_update_mode)(struct iwl_mvm *mvm,
                                  struct ieee80211_vif *vif);
+        int (*power_update_device_mode)(struct iwl_mvm *mvm);
         int (*power_disable)(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 #ifdef CONFIG_IWLWIFI_DEBUGFS
         int (*power_dbgfs_read)(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
@@ -241,12 +242,18 @@ enum iwl_mvm_smps_type_request {
 * @last_beacon_signal: last beacon rssi signal in dbm
 * @ave_beacon_signal: average beacon signal
 * @last_cqm_event: rssi of the last cqm event
+* @bt_coex_min_thold: minimum threshold for BT coex
+* @bt_coex_max_thold: maximum threshold for BT coex
+* @last_bt_coex_event: rssi of the last BT coex event
 */
 struct iwl_mvm_vif_bf_data {
         bool bf_enabled;
         bool ba_enabled;
         s8 ave_beacon_signal;
         s8 last_cqm_event;
+        s8 bt_coex_min_thold;
+        s8 bt_coex_max_thold;
+        s8 last_bt_coex_event;
 };
 
 /**
@@ -255,8 +262,8 @@ struct iwl_mvm_vif_bf_data {
  * @color: to solve races upon MAC addition and removal
  * @ap_sta_id: the sta_id of the AP - valid only if VIF type is STA
  * @uploaded: indicates the MAC context has been added to the device
- * @ap_active: indicates that ap context is configured, and that the interface
- *  should get quota etc.
+ * @ap_ibss_active: indicates that AP/IBSS is configured and that the interface
+ *      should get quota etc.
  * @monitor_active: indicates that monitor context is configured, and that the
  * interface should get quota etc.
  * @queue_params: QoS params for this MAC
@@ -272,7 +279,7 @@ struct iwl_mvm_vif {
         u8 ap_sta_id;
 
         bool uploaded;
-        bool ap_active;
+        bool ap_ibss_active;
         bool monitor_active;
         struct iwl_mvm_vif_bf_data bf_data;
 
@@ -306,6 +313,9 @@ struct iwl_mvm_vif {
 
         int tx_key_idx;
 
+        bool seqno_valid;
+        u16 seqno;
+
 #if IS_ENABLED(CONFIG_IPV6)
         /* IPv6 addresses for WoWLAN */
         struct in6_addr target_ipv6_addrs[IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_MAX];
@@ -333,6 +343,7 @@ iwl_mvm_vif_from_mac80211(struct ieee80211_vif *vif)
 enum iwl_scan_status {
         IWL_MVM_SCAN_NONE,
         IWL_MVM_SCAN_OS,
+        IWL_MVM_SCAN_SCHED,
 };
 
 /**
@@ -434,7 +445,7 @@ struct iwl_mvm {
 
         enum iwl_ucode_type cur_ucode;
         bool ucode_loaded;
-        bool init_ucode_run;
+        bool init_ucode_complete;
         u32 error_event_table;
         u32 log_event_table;
 
@@ -470,6 +481,9 @@ struct iwl_mvm {
         enum iwl_scan_status scan_status;
         struct iwl_scan_cmd *scan_cmd;
 
+        /* rx chain antennas set through debugfs for the scan command */
+        u8 scan_rx_ant;
+
         /* Internal station */
         struct iwl_mvm_int_sta aux_sta;
 
@@ -479,7 +493,8 @@ struct iwl_mvm {
 #ifdef CONFIG_IWLWIFI_DEBUGFS
         struct dentry *debugfs_dir;
         u32 dbgfs_sram_offset, dbgfs_sram_len;
-        bool prevent_power_down_d3;
+        bool disable_power_off;
+        bool disable_power_off_d3;
 #endif
 
         struct iwl_mvm_phy_ctxt phy_ctxts[NUM_PHY_CTX];
@@ -523,12 +538,23 @@ struct iwl_mvm {
         /* BT-Coex */
         u8 bt_kill_msk;
         struct iwl_bt_coex_profile_notif last_bt_notif;
+        struct iwl_bt_coex_ci_cmd last_bt_ci_cmd;
 
         /* Thermal Throttling and CTkill */
         struct iwl_mvm_tt_mgmt thermal_throttle;
         s32 temperature;        /* Celsius */
 
         const struct iwl_mvm_power_ops *pm_ops;
+
+#ifdef CONFIG_NL80211_TESTMODE
+        u32 noa_duration;
+        struct ieee80211_vif *noa_vif;
+#endif
+
+        /* Tx queues */
+        u8 aux_queue;
+        u8 first_agg_queue;
+        u8 last_agg_queue;
 };
 
 /* Extract MVM priv from op_mode and _hw */
@@ -570,6 +596,9 @@ int iwl_run_init_mvm_ucode(struct iwl_mvm *mvm, bool read_nvm);
 /* Utils */
 int iwl_mvm_legacy_rate_to_mac80211_idx(u32 rate_n_flags,
                                         enum ieee80211_band band);
+void iwl_mvm_hwrate_to_tx_rate(u32 rate_n_flags,
+                               enum ieee80211_band band,
+                               struct ieee80211_tx_rate *r);
 u8 iwl_mvm_mac80211_idx_to_hwrate(int rate_idx);
 void iwl_mvm_dump_nic_error_log(struct iwl_mvm *mvm);
 void iwl_mvm_dump_sram(struct iwl_mvm *mvm);
@@ -608,6 +637,7 @@ int iwl_mvm_rx_statistics(struct iwl_mvm *mvm,
 
 /* NVM */
 int iwl_nvm_init(struct iwl_mvm *mvm);
+int iwl_mvm_load_nvm_to_nic(struct iwl_mvm *mvm);
 
 int iwl_mvm_up(struct iwl_mvm *mvm);
 int iwl_mvm_load_d3_fw(struct iwl_mvm *mvm);
@@ -682,6 +712,23 @@ int iwl_mvm_rx_scan_complete(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
                              struct iwl_device_cmd *cmd);
 void iwl_mvm_cancel_scan(struct iwl_mvm *mvm);
 
+/* Scheduled scan */
+int iwl_mvm_rx_scan_offload_complete_notif(struct iwl_mvm *mvm,
+                                           struct iwl_rx_cmd_buffer *rxb,
+                                           struct iwl_device_cmd *cmd);
+int iwl_mvm_config_sched_scan(struct iwl_mvm *mvm,
+                              struct ieee80211_vif *vif,
+                              struct cfg80211_sched_scan_request *req,
+                              struct ieee80211_sched_scan_ies *ies);
+int iwl_mvm_config_sched_scan_profiles(struct iwl_mvm *mvm,
+                                       struct cfg80211_sched_scan_request *req);
+int iwl_mvm_sched_scan_start(struct iwl_mvm *mvm,
+                             struct cfg80211_sched_scan_request *req);
+void iwl_mvm_sched_scan_stop(struct iwl_mvm *mvm);
+int iwl_mvm_rx_sched_scan_results(struct iwl_mvm *mvm,
+                                  struct iwl_rx_cmd_buffer *rxb,
+                                  struct iwl_device_cmd *cmd);
+
 /* MVM debugfs */
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir);
@@ -720,6 +767,13 @@ static inline int iwl_mvm_power_disable(struct iwl_mvm *mvm,
         return mvm->pm_ops->power_disable(mvm, vif);
 }
 
+static inline int iwl_mvm_power_update_device_mode(struct iwl_mvm *mvm)
+{
+        if (mvm->pm_ops->power_update_device_mode)
+                return mvm->pm_ops->power_update_device_mode(mvm);
+        return 0;
+}
+
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 static inline int iwl_mvm_power_dbgfs_read(struct iwl_mvm *mvm,
                                             struct ieee80211_vif *vif,
@@ -745,6 +799,15 @@ void iwl_mvm_ipv6_addr_change(struct ieee80211_hw *hw,
 void iwl_mvm_set_default_unicast_key(struct ieee80211_hw *hw,
                                      struct ieee80211_vif *vif, int idx);
 extern const struct file_operations iwl_dbgfs_d3_test_ops;
+#ifdef CONFIG_PM_SLEEP
+void iwl_mvm_set_last_nonqos_seq(struct iwl_mvm *mvm,
+                                 struct ieee80211_vif *vif);
+#else
+static inline void
+iwl_mvm_set_last_nonqos_seq(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+{
+}
+#endif
 
 /* BT Coex */
 int iwl_send_bt_prio_tbl(struct iwl_mvm *mvm);
@@ -754,7 +817,20 @@ int iwl_mvm_rx_bt_coex_notif(struct iwl_mvm *mvm,
                              struct iwl_device_cmd *cmd);
 void iwl_mvm_bt_rssi_event(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
                            enum ieee80211_rssi_event rssi_event);
-void iwl_mvm_bt_coex_vif_assoc(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+void iwl_mvm_bt_coex_vif_change(struct iwl_mvm *mvm);
+u16 iwl_mvm_bt_coex_agg_time_limit(struct iwl_mvm *mvm,
+                                   struct ieee80211_sta *sta);
+bool iwl_mvm_bt_coex_is_mimo_allowed(struct iwl_mvm *mvm,
+                                     struct ieee80211_sta *sta);
+
+enum iwl_bt_kill_msk {
+        BT_KILL_MSK_DEFAULT,
+        BT_KILL_MSK_SCO_HID_A2DP,
+        BT_KILL_MSK_REDUCED_TXPOW,
+        BT_KILL_MSK_MAX,
+};
+extern const u32 iwl_bt_ack_kill_msk[BT_KILL_MSK_MAX];
+extern const u32 iwl_bt_cts_kill_msk[BT_KILL_MSK_MAX];
 
 /* beacon filtering */
 #ifdef CONFIG_IWLWIFI_DEBUGFS
diff --git a/drivers/net/wireless/iwlwifi/mvm/nvm.c b/drivers/net/wireless/iwlwifi/mvm/nvm.c
index edb94ea31654..2beffd028b67 100644
--- a/drivers/net/wireless/iwlwifi/mvm/nvm.c
+++ b/drivers/net/wireless/iwlwifi/mvm/nvm.c
@@ -77,7 +77,7 @@ static const int nvm_to_read[] = {
 
 /* Default NVM size to read */
 #define IWL_NVM_DEFAULT_CHUNK_SIZE (2*1024)
-#define IWL_MAX_NVM_SECTION_SIZE 6000
+#define IWL_MAX_NVM_SECTION_SIZE 7000
 
 #define NVM_WRITE_OPCODE 1
 #define NVM_READ_OPCODE 0
@@ -259,6 +259,8 @@ iwl_parse_nvm_sections(struct iwl_mvm *mvm)
 #define MAX_NVM_FILE_LEN        16384
 
 /*
+ * Reads external NVM from a file into mvm->nvm_sections
+ *
  * HOW TO CREATE THE NVM FILE FORMAT:
  * ------------------------------
  * 1. create hex file, format:
@@ -277,20 +279,23 @@ iwl_parse_nvm_sections(struct iwl_mvm *mvm)
  *
  * 4. save as "iNVM_xxx.bin" under /lib/firmware
  */
-static int iwl_mvm_load_external_nvm(struct iwl_mvm *mvm)
+static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 {
-        int ret, section_id, section_size;
+        int ret, section_size;
+        u16 section_id;
         const struct firmware *fw_entry;
         const struct {
                 __le16 word1;
                 __le16 word2;
                 u8 data[];
         } *file_sec;
-        const u8 *eof;
+        const u8 *eof, *temp;
 
 #define NVM_WORD1_LEN(x) (8 * (x & 0x03FF))
 #define NVM_WORD2_ID(x) (x >> 12)
 
+        IWL_DEBUG_EEPROM(mvm->trans->dev, "Read from external NVM\n");
+
         /*
          * Obtain NVM image via request_firmware. Since we already used
          * request_firmware_nowait() for the firmware binary load and only
@@ -362,12 +367,18 @@ static int iwl_mvm_load_external_nvm(struct iwl_mvm *mvm)
                         break;
                 }
 
-                ret = iwl_nvm_write_section(mvm, section_id, file_sec->data,
-                                            section_size);
-                if (ret < 0) {
-                        IWL_ERR(mvm, "iwl_mvm_send_cmd failed: %d\n", ret);
+                temp = kmemdup(file_sec->data, section_size, GFP_KERNEL);
+                if (!temp) {
+                        ret = -ENOMEM;
+                        break;
+                }
+                if (WARN_ON(section_id >= NVM_NUM_OF_SECTIONS)) {
+                        IWL_ERR(mvm, "Invalid NVM section ID\n");
+                        ret = -EINVAL;
                         break;
                 }
+                mvm->nvm_sections[section_id].data = temp;
+                mvm->nvm_sections[section_id].length = section_size;
 
                 /* advance to the next section */
                 file_sec = (void *)(file_sec->data + section_size);
@@ -377,6 +388,28 @@ out:
         return ret;
 }
 
+/* Loads the NVM data stored in mvm->nvm_sections into the NIC */
+int iwl_mvm_load_nvm_to_nic(struct iwl_mvm *mvm)
+{
+        int i, ret;
+        u16 section_id;
+        struct iwl_nvm_section *sections = mvm->nvm_sections;
+
+        IWL_DEBUG_EEPROM(mvm->trans->dev, "'Write to NVM\n");
+
+        for (i = 0; i < ARRAY_SIZE(nvm_to_read); i++) {
+                section_id = nvm_to_read[i];
+                ret = iwl_nvm_write_section(mvm, section_id,
+                                            sections[section_id].data,
+                                            sections[section_id].length);
+                if (ret < 0) {
+                        IWL_ERR(mvm, "iwl_mvm_send_cmd failed: %d\n", ret);
+                        break;
+                }
+        }
+        return ret;
+}
+
 int iwl_nvm_init(struct iwl_mvm *mvm)
 {
         int ret, i, section;
@@ -385,36 +418,36 @@ int iwl_nvm_init(struct iwl_mvm *mvm)
         /* load external NVM if configured */
         if (iwlwifi_mod_params.nvm_file) {
                 /* move to External NVM flow */
-                ret = iwl_mvm_load_external_nvm(mvm);
+                ret = iwl_mvm_read_external_nvm(mvm);
                 if (ret)
                         return ret;
-        }
-
-        /* Read From FW NVM */
-        IWL_DEBUG_EEPROM(mvm->trans->dev, "Read from NVM\n");
-
-        /* TODO: find correct NVM max size for a section */
-        nvm_buffer = kmalloc(mvm->cfg->base_params->eeprom_size,
-                             GFP_KERNEL);
-        if (!nvm_buffer)
-                return -ENOMEM;
-        for (i = 0; i < ARRAY_SIZE(nvm_to_read); i++) {
-                section = nvm_to_read[i];
-                /* we override the constness for initial read */
-                ret = iwl_nvm_read_section(mvm, section, nvm_buffer);
-                if (ret < 0)
-                        break;
-                temp = kmemdup(nvm_buffer, ret, GFP_KERNEL);
-                if (!temp) {
-                        ret = -ENOMEM;
-                        break;
+        } else {
+                /* Read From FW NVM */
+                IWL_DEBUG_EEPROM(mvm->trans->dev, "Read from NVM\n");
+
+                /* TODO: find correct NVM max size for a section */
+                nvm_buffer = kmalloc(mvm->cfg->base_params->eeprom_size,
+                                     GFP_KERNEL);
+                if (!nvm_buffer)
+                        return -ENOMEM;
+                for (i = 0; i < ARRAY_SIZE(nvm_to_read); i++) {
+                        section = nvm_to_read[i];
+                        /* we override the constness for initial read */
+                        ret = iwl_nvm_read_section(mvm, section, nvm_buffer);
+                        if (ret < 0)
+                                break;
+                        temp = kmemdup(nvm_buffer, ret, GFP_KERNEL);
+                        if (!temp) {
+                                ret = -ENOMEM;
+                                break;
+                        }
+                        mvm->nvm_sections[section].data = temp;
+                        mvm->nvm_sections[section].length = ret;
                 }
-                mvm->nvm_sections[section].data = temp;
-                mvm->nvm_sections[section].length = ret;
+                kfree(nvm_buffer);
+                if (ret < 0)
+                        return ret;
         }
-        kfree(nvm_buffer);
-        if (ret < 0)
-                return ret;
 
         mvm->nvm_data = iwl_parse_nvm_sections(mvm);
         if (!mvm->nvm_data)
diff --git a/drivers/net/wireless/iwlwifi/mvm/ops.c b/drivers/net/wireless/iwlwifi/mvm/ops.c
index 2fcc8ef88a68..59b7cb3c6134 100644
--- a/drivers/net/wireless/iwlwifi/mvm/ops.c
+++ b/drivers/net/wireless/iwlwifi/mvm/ops.c
@@ -224,6 +224,10 @@ static const struct iwl_rx_handlers iwl_mvm_rx_handlers[] = {
 
         RX_HANDLER(SCAN_REQUEST_CMD, iwl_mvm_rx_scan_response, false),
         RX_HANDLER(SCAN_COMPLETE_NOTIFICATION, iwl_mvm_rx_scan_complete, false),
+        RX_HANDLER(SCAN_OFFLOAD_COMPLETE,
+                   iwl_mvm_rx_scan_offload_complete_notif, false),
+        RX_HANDLER(MATCH_FOUND_NOTIFICATION, iwl_mvm_rx_sched_scan_results,
+                   false),
 
         RX_HANDLER(RADIO_VERSION_NOTIFICATION, iwl_mvm_rx_radio_ver, false),
         RX_HANDLER(CARD_STATE_NOTIFICATION, iwl_mvm_rx_card_state_notif, false),
@@ -249,6 +253,7 @@ static const char *iwl_mvm_cmd_strings[REPLY_MAX] = {
         CMD(TIME_EVENT_NOTIFICATION),
         CMD(BINDING_CONTEXT_CMD),
         CMD(TIME_QUOTA_CMD),
+        CMD(NON_QOS_TX_COUNTER_CMD),
         CMD(RADIO_VERSION_NOTIFICATION),
         CMD(SCAN_REQUEST_CMD),
         CMD(SCAN_ABORT_CMD),
@@ -260,10 +265,12 @@ static const char *iwl_mvm_cmd_strings[REPLY_MAX] = {
         CMD(CALIB_RES_NOTIF_PHY_DB),
         CMD(SET_CALIB_DEFAULT_CMD),
         CMD(CALIBRATION_COMPLETE_NOTIFICATION),
+        CMD(ADD_STA_KEY),
         CMD(ADD_STA),
         CMD(REMOVE_STA),
         CMD(LQ_CMD),
         CMD(SCAN_OFFLOAD_CONFIG_CMD),
+        CMD(MATCH_FOUND_NOTIFICATION),
         CMD(SCAN_OFFLOAD_REQUEST_CMD),
         CMD(SCAN_OFFLOAD_ABORT_CMD),
         CMD(SCAN_OFFLOAD_COMPLETE),
@@ -303,6 +310,7 @@ static const char *iwl_mvm_cmd_strings[REPLY_MAX] = {
         CMD(REPLY_BEACON_FILTERING_CMD),
         CMD(REPLY_THERMAL_MNG_BACKOFF),
         CMD(MAC_PM_POWER_TABLE),
+        CMD(BT_COEX_CI),
 };
 #undef CMD
 
@@ -344,6 +352,14 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 
         mvm->restart_fw = iwlwifi_mod_params.restart_fw ? -1 : 0;
 
+        mvm->aux_queue = 15;
+        mvm->first_agg_queue = 16;
+        mvm->last_agg_queue = mvm->cfg->base_params->num_of_queues - 1;
+        if (mvm->cfg->base_params->num_of_queues == 16) {
+                mvm->aux_queue = 11;
+                mvm->first_agg_queue = 12;
+        }
+
         mutex_init(&mvm->mutex);
         spin_lock_init(&mvm->async_handlers_lock);
         INIT_LIST_HEAD(&mvm->time_event_list);
@@ -401,24 +417,32 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
         IWL_INFO(mvm, "Detected %s, REV=0x%X\n",
                  mvm->cfg->name, mvm->trans->hw_rev);
 
-        err = iwl_trans_start_hw(mvm->trans);
-        if (err)
-                goto out_free;
-
         iwl_mvm_tt_initialize(mvm);
 
-        mutex_lock(&mvm->mutex);
-        err = iwl_run_init_mvm_ucode(mvm, true);
-        mutex_unlock(&mvm->mutex);
-        /* returns 0 if successful, 1 if success but in rfkill */
-        if (err < 0 && !iwlmvm_mod_params.init_dbg) {
-                IWL_ERR(mvm, "Failed to run INIT ucode: %d\n", err);
-                goto out_free;
-        }
+        /*
+         * If the NVM exists in an external file,
+         * there is no need to unnecessarily power up the NIC at driver load
+         */
+        if (iwlwifi_mod_params.nvm_file) {
+                        iwl_nvm_init(mvm);
+        } else {
+                err = iwl_trans_start_hw(mvm->trans);
+                if (err)
+                        goto out_free;
+
+                mutex_lock(&mvm->mutex);
+                err = iwl_run_init_mvm_ucode(mvm, true);
+                mutex_unlock(&mvm->mutex);
+                /* returns 0 if successful, 1 if success but in rfkill */
+                if (err < 0 && !iwlmvm_mod_params.init_dbg) {
+                        IWL_ERR(mvm, "Failed to run INIT ucode: %d\n", err);
+                        goto out_free;
+                }
 
-        /* Stop the hw after the ALIVE and NVM has been read */
-        if (!iwlmvm_mod_params.init_dbg)
-                iwl_trans_stop_hw(mvm->trans, false);
+                /* Stop the hw after the ALIVE and NVM has been read */
+                if (!iwlmvm_mod_params.init_dbg)
+                        iwl_trans_stop_hw(mvm->trans, false);
+        }
 
         scan_size = sizeof(struct iwl_scan_cmd) +
                 mvm->fw->ucode_capa.max_probe_length +
@@ -449,7 +473,8 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
  out_free:
         iwl_phy_db_free(mvm->phy_db);
         kfree(mvm->scan_cmd);
-        iwl_trans_stop_hw(trans, true);
+        if (!iwlwifi_mod_params.nvm_file)
+                iwl_trans_stop_hw(trans, true);
         ieee80211_free_hw(mvm->hw);
         return NULL;
 }
@@ -715,6 +740,9 @@ static void iwl_mvm_nic_restart(struct iwl_mvm *mvm)
                 case IWL_MVM_SCAN_OS:
                         ieee80211_scan_completed(mvm->hw, true);
                         break;
+                case IWL_MVM_SCAN_SCHED:
+                        ieee80211_sched_scan_stopped(mvm->hw);
+                        break;
                 }
 
                 if (mvm->restart_fw > 0)
diff --git a/drivers/net/wireless/iwlwifi/mvm/power.c b/drivers/net/wireless/iwlwifi/mvm/power.c
index d58e393324ef..550824aa84ea 100644
--- a/drivers/net/wireless/iwlwifi/mvm/power.c
+++ b/drivers/net/wireless/iwlwifi/mvm/power.c
@@ -300,11 +300,6 @@ static void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm,
         }
 
         if (cmd->flags & cpu_to_le16(POWER_FLAGS_ADVANCE_PM_ENA_MSK)) {
-                cmd->rx_data_timeout_uapsd =
-                        cpu_to_le32(IWL_MVM_UAPSD_RX_DATA_TIMEOUT);
-                cmd->tx_data_timeout_uapsd =
-                        cpu_to_le32(IWL_MVM_UAPSD_TX_DATA_TIMEOUT);
-
                 if (cmd->uapsd_ac_flags == (BIT(IEEE80211_AC_VO) |
                                             BIT(IEEE80211_AC_VI) |
                                             BIT(IEEE80211_AC_BE) |
@@ -319,10 +314,31 @@ static void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm,
                 }
 
                 cmd->uapsd_max_sp = IWL_UAPSD_MAX_SP;
-                cmd->heavy_tx_thld_packets =
-                        IWL_MVM_PS_HEAVY_TX_THLD_PACKETS;
-                cmd->heavy_rx_thld_packets =
-                        IWL_MVM_PS_HEAVY_RX_THLD_PACKETS;
+
+                if (mvm->cur_ucode == IWL_UCODE_WOWLAN || cmd->flags &
+                    cpu_to_le16(POWER_FLAGS_SNOOZE_ENA_MSK)) {
+                        cmd->rx_data_timeout_uapsd =
+                                cpu_to_le32(IWL_MVM_WOWLAN_PS_RX_DATA_TIMEOUT);
+                        cmd->tx_data_timeout_uapsd =
+                                cpu_to_le32(IWL_MVM_WOWLAN_PS_TX_DATA_TIMEOUT);
+                } else {
+                        cmd->rx_data_timeout_uapsd =
+                                cpu_to_le32(IWL_MVM_UAPSD_RX_DATA_TIMEOUT);
+                        cmd->tx_data_timeout_uapsd =
+                                cpu_to_le32(IWL_MVM_UAPSD_TX_DATA_TIMEOUT);
+                }
+
+                if (cmd->flags & cpu_to_le16(POWER_FLAGS_SNOOZE_ENA_MSK)) {
+                        cmd->heavy_tx_thld_packets =
+                                IWL_MVM_PS_SNOOZE_HEAVY_TX_THLD_PACKETS;
+                        cmd->heavy_rx_thld_packets =
+                                IWL_MVM_PS_SNOOZE_HEAVY_RX_THLD_PACKETS;
+                } else {
+                        cmd->heavy_tx_thld_packets =
+                                IWL_MVM_PS_HEAVY_TX_THLD_PACKETS;
+                        cmd->heavy_rx_thld_packets =
+                                IWL_MVM_PS_HEAVY_RX_THLD_PACKETS;
+                }
                 cmd->heavy_tx_thld_percentage =
                         IWL_MVM_PS_HEAVY_TX_THLD_PERCENT;
                 cmd->heavy_rx_thld_percentage =
@@ -430,6 +446,32 @@ static int iwl_mvm_power_mac_disable(struct iwl_mvm *mvm,
                                     sizeof(cmd), &cmd);
 }
 
+static int iwl_mvm_power_update_device(struct iwl_mvm *mvm)
+{
+        struct iwl_device_power_cmd cmd = {
+                .flags = cpu_to_le16(DEVICE_POWER_FLAGS_POWER_SAVE_ENA_MSK),
+        };
+
+        if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_DEVICE_PS_CMD))
+                return 0;
+
+        if (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_CAM)
+                cmd.flags |= cpu_to_le16(DEVICE_POWER_FLAGS_CAM_MSK);
+
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+        if ((mvm->cur_ucode == IWL_UCODE_WOWLAN) ? mvm->disable_power_off_d3 :
+            mvm->disable_power_off)
+                cmd.flags &=
+                        cpu_to_le16(~DEVICE_POWER_FLAGS_POWER_SAVE_ENA_MSK);
+#endif
+        IWL_DEBUG_POWER(mvm,
+                        "Sending device power command with flags = 0x%X\n",
+                        cmd.flags);
+
+        return iwl_mvm_send_cmd_pdu(mvm, POWER_TABLE_CMD, CMD_SYNC, sizeof(cmd),
+                                    &cmd);
+}
+
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 static int iwl_mvm_power_mac_dbgfs_read(struct iwl_mvm *mvm,
                                         struct ieee80211_vif *vif, char *buf,
@@ -440,10 +482,11 @@ static int iwl_mvm_power_mac_dbgfs_read(struct iwl_mvm *mvm,
 
         iwl_mvm_power_build_cmd(mvm, vif, &cmd);
 
-        pos += scnprintf(buf+pos, bufsz-pos, "disable_power_off = %d\n",
-                         (cmd.flags &
-                         cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK)) ?
-                         0 : 1);
+        if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_DEVICE_PS_CMD))
+                pos += scnprintf(buf+pos, bufsz-pos, "disable_power_off = %d\n",
+                                 (cmd.flags &
+                                 cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK)) ?
+                                 0 : 1);
         pos += scnprintf(buf+pos, bufsz-pos, "power_scheme = %d\n",
                          iwlmvm_mod_params.power_scheme);
         pos += scnprintf(buf+pos, bufsz-pos, "flags = 0x%x\n",
@@ -609,6 +652,7 @@ int iwl_mvm_update_beacon_filter(struct iwl_mvm *mvm,
 
 const struct iwl_mvm_power_ops pm_mac_ops = {
         .power_update_mode = iwl_mvm_power_mac_update_mode,
+        .power_update_device_mode = iwl_mvm_power_update_device,
         .power_disable = iwl_mvm_power_mac_disable,
 #ifdef CONFIG_IWLWIFI_DEBUGFS
         .power_dbgfs_read = iwl_mvm_power_mac_dbgfs_read,
diff --git a/drivers/net/wireless/iwlwifi/mvm/quota.c b/drivers/net/wireless/iwlwifi/mvm/quota.c
index 5c6ae16ec52b..17e2bc827f9a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/quota.c
+++ b/drivers/net/wireless/iwlwifi/mvm/quota.c
@@ -110,7 +110,8 @@ static void iwl_mvm_quota_iterator(void *_data, u8 *mac,
                         data->n_interfaces[id]++;
                 break;
         case NL80211_IFTYPE_AP:
-                if (mvmvif->ap_active)
+        case NL80211_IFTYPE_ADHOC:
+                if (mvmvif->ap_ibss_active)
                         data->n_interfaces[id]++;
                 break;
         case NL80211_IFTYPE_MONITOR:
@@ -119,16 +120,45 @@ static void iwl_mvm_quota_iterator(void *_data, u8 *mac,
                 break;
         case NL80211_IFTYPE_P2P_DEVICE:
                 break;
-        case NL80211_IFTYPE_ADHOC:
-                if (vif->bss_conf.ibss_joined)
-                        data->n_interfaces[id]++;
-                break;
         default:
                 WARN_ON_ONCE(1);
                 break;
         }
 }
 
+static void iwl_mvm_adjust_quota_for_noa(struct iwl_mvm *mvm,
+                                         struct iwl_time_quota_cmd *cmd)
+{
+#ifdef CONFIG_NL80211_TESTMODE
+        struct iwl_mvm_vif *mvmvif;
+        int i, phy_id = -1, beacon_int = 0;
+
+        if (!mvm->noa_duration || !mvm->noa_vif)
+                return;
+
+        mvmvif = iwl_mvm_vif_from_mac80211(mvm->noa_vif);
+        if (!mvmvif->ap_ibss_active)
+                return;
+
+        phy_id = mvmvif->phy_ctxt->id;
+        beacon_int = mvm->noa_vif->bss_conf.beacon_int;
+
+        for (i = 0; i < MAX_BINDINGS; i++) {
+                u32 id_n_c = le32_to_cpu(cmd->quotas[i].id_and_color);
+                u32 id = (id_n_c & FW_CTXT_ID_MSK) >> FW_CTXT_ID_POS;
+                u32 quota = le32_to_cpu(cmd->quotas[i].quota);
+
+                if (id != phy_id)
+                        continue;
+
+                quota *= (beacon_int - mvm->noa_duration);
+                quota /= beacon_int;
+
+                cmd->quotas[i].quota = cpu_to_le32(quota);
+        }
+#endif
+}
+
 int iwl_mvm_update_quotas(struct iwl_mvm *mvm, struct ieee80211_vif *newvif)
 {
         struct iwl_time_quota_cmd cmd = {};
@@ -196,6 +226,8 @@ int iwl_mvm_update_quotas(struct iwl_mvm *mvm, struct ieee80211_vif *newvif)
         /* Give the remainder of the session to the first binding */
         le32_add_cpu(&cmd.quotas[0].quota, quota_rem);
 
+        iwl_mvm_adjust_quota_for_noa(mvm, &cmd);
+
         ret = iwl_mvm_send_cmd_pdu(mvm, TIME_QUOTA_CMD, CMD_SYNC,
                                    sizeof(cmd), &cmd);
         if (ret)
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.c b/drivers/net/wireless/iwlwifi/mvm/rs.c
index 4ffaa3fa153f..a0b4cc8d9c3b 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.c
@@ -82,13 +82,24 @@ static const u8 ant_toggle_lookup[] = {
         [ANT_ABC] = ANT_ABC,
 };
 
-#define IWL_DECLARE_RATE_INFO(r, s, rp, rn)                    \
-        [IWL_RATE_##r##M_INDEX] = { IWL_RATE_##r##M_PLCP,      \
-                                    IWL_RATE_SISO_##s##M_PLCP, \
-                                    IWL_RATE_MIMO2_##s##M_PLCP,\
-                                    IWL_RATE_##rp##M_INDEX,    \
+#define IWL_DECLARE_RATE_INFO(r, s, rp, rn)                           \
+        [IWL_RATE_##r##M_INDEX] = { IWL_RATE_##r##M_PLCP,             \
+                                    IWL_RATE_HT_SISO_MCS_##s##_PLCP,  \
+                                    IWL_RATE_HT_MIMO2_MCS_##s##_PLCP, \
+                                    IWL_RATE_VHT_SISO_MCS_##s##_PLCP, \
+                                    IWL_RATE_VHT_MIMO2_MCS_##s##_PLCP,\
+                                    IWL_RATE_##rp##M_INDEX,           \
                                     IWL_RATE_##rn##M_INDEX }
 
+#define IWL_DECLARE_MCS_RATE(s)                                           \
+        [IWL_RATE_MCS_##s##_INDEX] = { IWL_RATE_INVM_PLCP,                \
+                                       IWL_RATE_HT_SISO_MCS_##s##_PLCP,   \
+                                       IWL_RATE_HT_MIMO2_MCS_##s##_PLCP,  \
+                                       IWL_RATE_VHT_SISO_MCS_##s##_PLCP,  \
+                                       IWL_RATE_VHT_MIMO2_MCS_##s##_PLCP, \
+                                       IWL_RATE_INVM_INDEX,               \
+                                       IWL_RATE_INVM_INDEX }
+
 /*
  * Parameter order:
  *   rate, ht rate, prev rate, next rate
@@ -102,16 +113,17 @@ static const struct iwl_rs_rate_info iwl_rates[IWL_RATE_COUNT] = {
         IWL_DECLARE_RATE_INFO(2, INV, 1, 5),     /*  2mbps */
         IWL_DECLARE_RATE_INFO(5, INV, 2, 11),    /*5.5mbps */
         IWL_DECLARE_RATE_INFO(11, INV, 9, 12),   /* 11mbps */
-        IWL_DECLARE_RATE_INFO(6, 6, 5, 11),      /*  6mbps */
-        IWL_DECLARE_RATE_INFO(9, 6, 6, 11),      /*  9mbps */
-        IWL_DECLARE_RATE_INFO(12, 12, 11, 18),   /* 12mbps */
-        IWL_DECLARE_RATE_INFO(18, 18, 12, 24),   /* 18mbps */
-        IWL_DECLARE_RATE_INFO(24, 24, 18, 36),   /* 24mbps */
-        IWL_DECLARE_RATE_INFO(36, 36, 24, 48),   /* 36mbps */
-        IWL_DECLARE_RATE_INFO(48, 48, 36, 54),   /* 48mbps */
-        IWL_DECLARE_RATE_INFO(54, 54, 48, INV),  /* 54mbps */
-        IWL_DECLARE_RATE_INFO(60, 60, 48, INV),  /* 60mbps */
-        /* FIXME:RS:          ^^    should be INV (legacy) */
+        IWL_DECLARE_RATE_INFO(6, 0, 5, 11),      /*  6mbps ; MCS 0 */
+        IWL_DECLARE_RATE_INFO(9, INV, 6, 11),    /*  9mbps */
+        IWL_DECLARE_RATE_INFO(12, 1, 11, 18),    /* 12mbps ; MCS 1 */
+        IWL_DECLARE_RATE_INFO(18, 2, 12, 24),    /* 18mbps ; MCS 2 */
+        IWL_DECLARE_RATE_INFO(24, 3, 18, 36),    /* 24mbps ; MCS 3 */
+        IWL_DECLARE_RATE_INFO(36, 4, 24, 48),    /* 36mbps ; MCS 4 */
+        IWL_DECLARE_RATE_INFO(48, 5, 36, 54),    /* 48mbps ; MCS 5 */
+        IWL_DECLARE_RATE_INFO(54, 6, 48, INV),   /* 54mbps ; MCS 6 */
+        IWL_DECLARE_MCS_RATE(7),                 /* MCS 7 */
+        IWL_DECLARE_MCS_RATE(8),                 /* MCS 8 */
+        IWL_DECLARE_MCS_RATE(9),                 /* MCS 9 */
 };
 
 static inline u8 rs_extract_rate(u32 rate_n_flags)
@@ -124,26 +136,30 @@ static int iwl_hwrate_to_plcp_idx(u32 rate_n_flags)
 {
         int idx = 0;
 
-        /* HT rate format */
         if (rate_n_flags & RATE_MCS_HT_MSK) {
-                idx = rs_extract_rate(rate_n_flags);
-
-                WARN_ON_ONCE(idx >= IWL_RATE_MIMO3_6M_PLCP);
-                if (idx >= IWL_RATE_MIMO2_6M_PLCP)
-                        idx = idx - IWL_RATE_MIMO2_6M_PLCP;
+                idx = rate_n_flags & RATE_HT_MCS_RATE_CODE_MSK;
+                idx += IWL_RATE_MCS_0_INDEX;
 
-                idx += IWL_FIRST_OFDM_RATE;
-                /* skip 9M not supported in ht*/
+                /* skip 9M not supported in HT*/
                 if (idx >= IWL_RATE_9M_INDEX)
                         idx += 1;
-                if ((idx >= IWL_FIRST_OFDM_RATE) && (idx <= IWL_LAST_OFDM_RATE))
+                if ((idx >= IWL_FIRST_HT_RATE) && (idx <= IWL_LAST_HT_RATE))
                         return idx;
+        } else if (rate_n_flags & RATE_MCS_VHT_MSK) {
+                idx = rate_n_flags & RATE_VHT_MCS_RATE_CODE_MSK;
+                idx += IWL_RATE_MCS_0_INDEX;
 
-        /* legacy rate format, search for match in table */
+                /* skip 9M not supported in VHT*/
+                if (idx >= IWL_RATE_9M_INDEX)
+                        idx++;
+                if ((idx >= IWL_FIRST_VHT_RATE) && (idx <= IWL_LAST_VHT_RATE))
+                        return idx;
         } else {
+                /* legacy rate format, search for match in table */
+
+                u8 legacy_rate = rs_extract_rate(rate_n_flags);
                 for (idx = 0; idx < ARRAY_SIZE(iwl_rates); idx++)
-                        if (iwl_rates[idx].plcp ==
-                                        rs_extract_rate(rate_n_flags))
+                        if (iwl_rates[idx].plcp == legacy_rate)
                                 return idx;
         }
 
@@ -155,6 +171,7 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
                                    struct ieee80211_sta *sta,
                                    struct iwl_lq_sta *lq_sta);
 static void rs_fill_link_cmd(struct iwl_mvm *mvm,
+                             struct ieee80211_sta *sta,
                              struct iwl_lq_sta *lq_sta, u32 rate_n_flags);
 static void rs_stay_in_table(struct iwl_lq_sta *lq_sta, bool force_search);
 
@@ -180,35 +197,52 @@ static void rs_dbgfs_set_mcs(struct iwl_lq_sta *lq_sta,
  */
 
 static s32 expected_tpt_legacy[IWL_RATE_COUNT] = {
-        7, 13, 35, 58, 40, 57, 72, 98, 121, 154, 177, 186, 0
+        7, 13, 35, 58, 40, 57, 72, 98, 121, 154, 177, 186, 0, 0, 0
 };
 
-static s32 expected_tpt_siso20MHz[4][IWL_RATE_COUNT] = {
-        {0, 0, 0, 0, 42, 0,  76, 102, 124, 159, 183, 193, 202}, /* Norm */
-        {0, 0, 0, 0, 46, 0,  82, 110, 132, 168, 192, 202, 210}, /* SGI */
-        {0, 0, 0, 0, 47, 0,  91, 133, 171, 242, 305, 334, 362}, /* AGG */
-        {0, 0, 0, 0, 52, 0, 101, 145, 187, 264, 330, 361, 390}, /* AGG+SGI */
+/* Expected TpT tables. 4 indexes:
+ * 0 - NGI, 1 - SGI, 2 - AGG+NGI, 3 - AGG+SGI
+ */
+static s32 expected_tpt_siso_20MHz[4][IWL_RATE_COUNT] = {
+        {0, 0, 0, 0, 42, 0,  76, 102, 124, 159, 183, 193, 202, 216, 0},
+        {0, 0, 0, 0, 46, 0,  82, 110, 132, 168, 192, 202, 210, 225, 0},
+        {0, 0, 0, 0, 49, 0,  97, 145, 192, 285, 375, 420, 464, 551, 0},
+        {0, 0, 0, 0, 54, 0, 108, 160, 213, 315, 415, 465, 513, 608, 0},
 };
 
-static s32 expected_tpt_siso40MHz[4][IWL_RATE_COUNT] = {
-        {0, 0, 0, 0,  77, 0, 127, 160, 184, 220, 242, 250, 257}, /* Norm */
-        {0, 0, 0, 0,  83, 0, 135, 169, 193, 229, 250, 257, 264}, /* SGI */
-        {0, 0, 0, 0,  94, 0, 177, 249, 313, 423, 512, 550, 586}, /* AGG */
-        {0, 0, 0, 0, 104, 0, 193, 270, 338, 454, 545, 584, 620}, /* AGG+SGI */
+static s32 expected_tpt_siso_40MHz[4][IWL_RATE_COUNT] = {
+        {0, 0, 0, 0,  77, 0, 127, 160, 184, 220, 242, 250,  257,  269,  275},
+        {0, 0, 0, 0,  83, 0, 135, 169, 193, 229, 250, 257,  264,  275,  280},
+        {0, 0, 0, 0, 101, 0, 199, 295, 389, 570, 744, 828,  911, 1070, 1173},
+        {0, 0, 0, 0, 112, 0, 220, 326, 429, 629, 819, 912, 1000, 1173, 1284},
+};
+
+static s32 expected_tpt_siso_80MHz[4][IWL_RATE_COUNT] = {
+        {0, 0, 0, 0, 130, 0, 191, 223, 244,  273,  288,  294,  298,  305,  308},
+        {0, 0, 0, 0, 138, 0, 200, 231, 251,  279,  293,  298,  302,  308,  312},
+        {0, 0, 0, 0, 217, 0, 429, 634, 834, 1220, 1585, 1760, 1931, 2258, 2466},
+        {0, 0, 0, 0, 241, 0, 475, 701, 921, 1343, 1741, 1931, 2117, 2468, 2691},
 };
 
 static s32 expected_tpt_mimo2_20MHz[4][IWL_RATE_COUNT] = {
-        {0, 0, 0, 0,  74, 0, 123, 155, 179, 214, 236, 244, 251}, /* Norm */
-        {0, 0, 0, 0,  81, 0, 131, 164, 188, 223, 243, 251, 257}, /* SGI */
-        {0, 0, 0, 0,  89, 0, 167, 235, 296, 402, 488, 526, 560}, /* AGG */
-        {0, 0, 0, 0,  97, 0, 182, 255, 320, 431, 520, 558, 593}, /* AGG+SGI*/
+        {0, 0, 0, 0,  74, 0, 123, 155, 179, 213, 235, 243, 250,  261, 0},
+        {0, 0, 0, 0,  81, 0, 131, 164, 187, 221, 242, 250, 256,  267, 0},
+        {0, 0, 0, 0,  98, 0, 193, 286, 375, 550, 718, 799, 878, 1032, 0},
+        {0, 0, 0, 0, 109, 0, 214, 316, 414, 607, 790, 879, 965, 1132, 0},
 };
 
 static s32 expected_tpt_mimo2_40MHz[4][IWL_RATE_COUNT] = {
-        {0, 0, 0, 0, 123, 0, 182, 214, 235, 264, 279, 285, 289}, /* Norm */
-        {0, 0, 0, 0, 131, 0, 191, 222, 242, 270, 284, 289, 293}, /* SGI */
-        {0, 0, 0, 0, 171, 0, 305, 410, 496, 634, 731, 771, 805}, /* AGG */
-        {0, 0, 0, 0, 186, 0, 329, 439, 527, 667, 764, 803, 838}, /* AGG+SGI */
+        {0, 0, 0, 0, 123, 0, 182, 214, 235,  264,  279,  285,  289,  296,  300},
+        {0, 0, 0, 0, 131, 0, 191, 222, 242,  270,  284,  289,  293,  300,  303},
+        {0, 0, 0, 0, 200, 0, 390, 571, 741, 1067, 1365, 1505, 1640, 1894, 2053},
+        {0, 0, 0, 0, 221, 0, 430, 630, 816, 1169, 1490, 1641, 1784, 2053, 2221},
+};
+
+static s32 expected_tpt_mimo2_80MHz[4][IWL_RATE_COUNT] = {
+        {0, 0, 0, 0, 182, 0, 240,  264,  278,  299,  308,  311,  313,  317,  319},
+        {0, 0, 0, 0, 190, 0, 247,  269,  282,  302,  310,  313,  315,  319,  320},
+        {0, 0, 0, 0, 428, 0, 833, 1215, 1577, 2254, 2863, 3147, 3418, 3913, 4219},
+        {0, 0, 0, 0, 474, 0, 920, 1338, 1732, 2464, 3116, 3418, 3705, 4225, 4545},
 };
 
 /* mbps, mcs */
@@ -263,7 +297,7 @@ static void rs_program_fix_rate(struct iwl_mvm *mvm,
                        lq_sta->lq.sta_id, lq_sta->dbg_fixed_rate);
 
         if (lq_sta->dbg_fixed_rate) {
-                rs_fill_link_cmd(NULL, lq_sta, lq_sta->dbg_fixed_rate);
+                rs_fill_link_cmd(NULL, NULL, lq_sta, lq_sta->dbg_fixed_rate);
                 iwl_mvm_send_lq_cmd(lq_sta->drv, &lq_sta->lq, CMD_ASYNC, false);
         }
 }
@@ -275,17 +309,6 @@ static int rs_tl_turn_on_agg_for_tid(struct iwl_mvm *mvm,
 {
         int ret = -EAGAIN;
 
-        /*
-         * Don't create TX aggregation sessions when in high
-         * BT traffic, as they would just be disrupted by BT.
-         */
-        if (BT_MBOX_MSG(&mvm->last_bt_notif, 3, TRAFFIC_LOAD) >= 2) {
-                IWL_DEBUG_COEX(mvm, "BT traffic (%d), no aggregation allowed\n",
-                               BT_MBOX_MSG(&mvm->last_bt_notif,
-                                           3, TRAFFIC_LOAD));
-                return ret;
-        }
-
         IWL_DEBUG_HT(mvm, "Starting Tx agg: STA: %pM tid: %d\n",
                      sta->addr, tid);
         ret = ieee80211_start_tx_ba_session(sta, tid, 5000);
@@ -416,49 +439,54 @@ static int rs_collect_tx_data(struct iwl_scale_tbl_info *tbl,
  */
 /* FIXME:RS:remove this function and put the flags statically in the table */
 static u32 rate_n_flags_from_tbl(struct iwl_mvm *mvm,
-                                 struct iwl_scale_tbl_info *tbl,
-                                 int index, u8 use_green)
+                                 struct iwl_scale_tbl_info *tbl, int index)
 {
         u32 rate_n_flags = 0;
 
+        rate_n_flags |= ((tbl->ant_type << RATE_MCS_ANT_POS) &
+                         RATE_MCS_ANT_ABC_MSK);
+
         if (is_legacy(tbl->lq_type)) {
-                rate_n_flags = iwl_rates[index].plcp;
+                rate_n_flags |= iwl_rates[index].plcp;
                 if (index >= IWL_FIRST_CCK_RATE && index <= IWL_LAST_CCK_RATE)
                         rate_n_flags |= RATE_MCS_CCK_MSK;
-        } else if (is_Ht(tbl->lq_type)) {
-                if (index > IWL_LAST_OFDM_RATE) {
+                return rate_n_flags;
+        }
+
+        if (is_ht(tbl->lq_type)) {
+                if (index < IWL_FIRST_HT_RATE || index > IWL_LAST_HT_RATE) {
                         IWL_ERR(mvm, "Invalid HT rate index %d\n", index);
-                        index = IWL_LAST_OFDM_RATE;
+                        index = IWL_LAST_HT_RATE;
                 }
-                rate_n_flags = RATE_MCS_HT_MSK;
+                rate_n_flags |= RATE_MCS_HT_MSK;
 
-                if (is_siso(tbl->lq_type))
-                        rate_n_flags |= iwl_rates[index].plcp_siso;
-                else if (is_mimo2(tbl->lq_type))
-                        rate_n_flags |= iwl_rates[index].plcp_mimo2;
+                if (is_ht_siso(tbl->lq_type))
+                        rate_n_flags |= iwl_rates[index].plcp_ht_siso;
+                else if (is_ht_mimo2(tbl->lq_type))
+                        rate_n_flags |= iwl_rates[index].plcp_ht_mimo2;
                 else
                         WARN_ON_ONCE(1);
+        } else if (is_vht(tbl->lq_type)) {
+                if (index < IWL_FIRST_VHT_RATE || index > IWL_LAST_VHT_RATE) {
+                        IWL_ERR(mvm, "Invalid VHT rate index %d\n", index);
+                        index = IWL_LAST_VHT_RATE;
+                }
+                rate_n_flags |= RATE_MCS_VHT_MSK;
+                if (is_vht_siso(tbl->lq_type))
+                        rate_n_flags |= iwl_rates[index].plcp_vht_siso;
+                else if (is_vht_mimo2(tbl->lq_type))
+                        rate_n_flags |= iwl_rates[index].plcp_vht_mimo2;
+                else
+                        WARN_ON_ONCE(1);
+
         } else {
                 IWL_ERR(mvm, "Invalid tbl->lq_type %d\n", tbl->lq_type);
         }
 
-        rate_n_flags |= ((tbl->ant_type << RATE_MCS_ANT_POS) &
-                                                     RATE_MCS_ANT_ABC_MSK);
-
-        if (is_Ht(tbl->lq_type)) {
-                if (tbl->is_ht40)
-                        rate_n_flags |= RATE_MCS_CHAN_WIDTH_40;
-                if (tbl->is_SGI)
-                        rate_n_flags |= RATE_MCS_SGI_MSK;
-
-                if (use_green) {
-                        rate_n_flags |= RATE_HT_MCS_GF_MSK;
-                        if (is_siso(tbl->lq_type) && tbl->is_SGI) {
-                                rate_n_flags &= ~RATE_MCS_SGI_MSK;
-                                IWL_ERR(mvm, "GF was set with SGI:SISO\n");
-                        }
-                }
-        }
+        rate_n_flags |= tbl->bw;
+        if (tbl->is_SGI)
+                rate_n_flags |= RATE_MCS_SGI_MSK;
+
         return rate_n_flags;
 }
 
@@ -473,7 +501,7 @@ static int rs_get_tbl_info_from_mcs(const u32 rate_n_flags,
 {
         u32 ant_msk = (rate_n_flags & RATE_MCS_ANT_ABC_MSK);
         u8 num_of_ant = get_num_of_ant_from_rate(rate_n_flags);
-        u8 mcs;
+        u8 nss;
 
         memset(tbl, 0, offsetof(struct iwl_scale_tbl_info, win));
         *rate_idx = iwl_hwrate_to_plcp_idx(rate_n_flags);
@@ -483,41 +511,62 @@ static int rs_get_tbl_info_from_mcs(const u32 rate_n_flags,
                 return -EINVAL;
         }
         tbl->is_SGI = 0;        /* default legacy setup */
-        tbl->is_ht40 = 0;
+        tbl->bw = 0;
         tbl->ant_type = (ant_msk >> RATE_MCS_ANT_POS);
         tbl->lq_type = LQ_NONE;
         tbl->max_search = IWL_MAX_SEARCH;
 
-        /* legacy rate format */
-        if (!(rate_n_flags & RATE_MCS_HT_MSK)) {
+        /* Legacy */
+        if (!(rate_n_flags & RATE_MCS_HT_MSK) &&
+            !(rate_n_flags & RATE_MCS_VHT_MSK)) {
                 if (num_of_ant == 1) {
                         if (band == IEEE80211_BAND_5GHZ)
-                                tbl->lq_type = LQ_A;
+                                tbl->lq_type = LQ_LEGACY_A;
                         else
-                                tbl->lq_type = LQ_G;
+                                tbl->lq_type = LQ_LEGACY_G;
                 }
-        /* HT rate format */
-        } else {
-                if (rate_n_flags & RATE_MCS_SGI_MSK)
-                        tbl->is_SGI = 1;
-
-                if (rate_n_flags & RATE_MCS_CHAN_WIDTH_40) /* TODO */
-                        tbl->is_ht40 = 1;
-
-                mcs = rs_extract_rate(rate_n_flags);
-
-                /* SISO */
-                if (mcs <= IWL_RATE_SISO_60M_PLCP) {
-                        if (num_of_ant == 1)
-                                tbl->lq_type = LQ_SISO; /*else NONE*/
-                /* MIMO2 */
-                } else if (mcs <= IWL_RATE_MIMO2_60M_PLCP) {
-                        if (num_of_ant == 2)
-                                tbl->lq_type = LQ_MIMO2;
+
+                return 0;
+        }
+
+        /* HT or VHT */
+        if (rate_n_flags & RATE_MCS_SGI_MSK)
+                tbl->is_SGI = 1;
+
+        tbl->bw = rate_n_flags & RATE_MCS_CHAN_WIDTH_MSK;
+
+        if (rate_n_flags & RATE_MCS_HT_MSK) {
+                nss = ((rate_n_flags & RATE_HT_MCS_NSS_MSK) >>
+                       RATE_HT_MCS_NSS_POS) + 1;
+
+                if (nss == 1) {
+                        tbl->lq_type = LQ_HT_SISO;
+                        WARN_ON_ONCE(num_of_ant != 1);
+                } else if (nss == 2) {
+                        tbl->lq_type = LQ_HT_MIMO2;
+                        WARN_ON_ONCE(num_of_ant != 2);
+                } else {
+                        WARN_ON_ONCE(1);
+                }
+        } else if (rate_n_flags & RATE_MCS_VHT_MSK) {
+                nss = ((rate_n_flags & RATE_VHT_MCS_NSS_MSK) >>
+                       RATE_VHT_MCS_NSS_POS) + 1;
+
+                if (nss == 1) {
+                        tbl->lq_type = LQ_VHT_SISO;
+                        WARN_ON_ONCE(num_of_ant != 1);
+                } else if (nss == 2) {
+                        tbl->lq_type = LQ_VHT_MIMO2;
+                        WARN_ON_ONCE(num_of_ant != 2);
                 } else {
-                        WARN_ON_ONCE(num_of_ant == 3);
+                        WARN_ON_ONCE(1);
                 }
         }
+
+        WARN_ON_ONCE(tbl->bw == RATE_MCS_CHAN_WIDTH_160);
+        WARN_ON_ONCE(tbl->bw == RATE_MCS_CHAN_WIDTH_80 &&
+                     !is_vht(tbl->lq_type));
+
         return 0;
 }
 
@@ -550,22 +599,6 @@ static int rs_toggle_antenna(u32 valid_ant, u32 *rate_n_flags,
 }
 
 /**
- * Green-field mode is valid if the station supports it and
- * there are no non-GF stations present in the BSS.
- */
-static bool rs_use_green(struct ieee80211_sta *sta)
-{
-        /*
-         * There's a bug somewhere in this code that causes the
-         * scaling to get stuck because GF+SGI can't be combined
-         * in SISO rates. Until we find that bug, disable GF, it
-         * has only limited benefit and we still interoperate with
-         * GF APs since we can always receive GF transmissions.
-         */
-        return false;
-}
-
-/**
  * rs_get_supported_rates - get the available rates
  *
  * if management frame or broadcast frame only return
@@ -576,16 +609,15 @@ static u16 rs_get_supported_rates(struct iwl_lq_sta *lq_sta,
                                   struct ieee80211_hdr *hdr,
                                   enum iwl_table_type rate_type)
 {
-        if (is_legacy(rate_type)) {
+        if (is_legacy(rate_type))
                 return lq_sta->active_legacy_rate;
-        } else {
-                if (is_siso(rate_type))
-                        return lq_sta->active_siso_rate;
-                else {
-                        WARN_ON_ONCE(!is_mimo2(rate_type));
-                        return lq_sta->active_mimo2_rate;
-                }
-        }
+        else if (is_siso(rate_type))
+                return lq_sta->active_siso_rate;
+        else if (is_mimo2(rate_type))
+                return lq_sta->active_mimo2_rate;
+
+        WARN_ON_ONCE(1);
+        return 0;
 }
 
 static u16 rs_get_adjacent_rate(struct iwl_mvm *mvm, u8 index, u16 rate_mask,
@@ -652,7 +684,6 @@ static u32 rs_get_lower_rate(struct iwl_lq_sta *lq_sta,
         u16 rate_mask;
         u16 high_low;
         u8 switch_to_legacy = 0;
-        u8 is_green = lq_sta->is_green;
         struct iwl_mvm *mvm = lq_sta->drv;
 
         /* check if we need to switch from HT to legacy rates.
@@ -662,15 +693,15 @@ static u32 rs_get_lower_rate(struct iwl_lq_sta *lq_sta,
                 switch_to_legacy = 1;
                 scale_index = rs_ht_to_legacy[scale_index];
                 if (lq_sta->band == IEEE80211_BAND_5GHZ)
-                        tbl->lq_type = LQ_A;
+                        tbl->lq_type = LQ_LEGACY_A;
                 else
-                        tbl->lq_type = LQ_G;
+                        tbl->lq_type = LQ_LEGACY_G;
 
                 if (num_of_ant(tbl->ant_type) > 1)
                         tbl->ant_type =
                             first_antenna(iwl_fw_valid_tx_ant(mvm->fw));
 
-                tbl->is_ht40 = 0;
+                tbl->bw = 0;
                 tbl->is_SGI = 0;
                 tbl->max_search = IWL_MAX_SEARCH;
         }
@@ -701,7 +732,7 @@ static u32 rs_get_lower_rate(struct iwl_lq_sta *lq_sta,
                 low = scale_index;
 
 out:
-        return rate_n_flags_from_tbl(lq_sta->drv, tbl, low, is_green);
+        return rate_n_flags_from_tbl(lq_sta->drv, tbl, low);
 }
 
 /*
@@ -714,6 +745,18 @@ static bool table_type_matches(struct iwl_scale_tbl_info *a,
                 (a->is_SGI == b->is_SGI);
 }
 
+static u32 rs_ch_width_from_mac_flags(enum mac80211_rate_control_flags flags)
+{
+        if (flags & IEEE80211_TX_RC_40_MHZ_WIDTH)
+                return RATE_MCS_CHAN_WIDTH_40;
+        else if (flags & IEEE80211_TX_RC_80_MHZ_WIDTH)
+                return RATE_MCS_CHAN_WIDTH_80;
+        else if (flags & IEEE80211_TX_RC_160_MHZ_WIDTH)
+                return RATE_MCS_CHAN_WIDTH_160;
+
+        return RATE_MCS_CHAN_WIDTH_20;
+}
+
 /*
  * mac80211 sends us Tx status
  */
@@ -783,16 +826,23 @@ static void rs_tx_status(void *mvm_r, struct ieee80211_supported_band *sband,
                  */
                 if (info->band == IEEE80211_BAND_2GHZ)
                         mac_index += IWL_FIRST_OFDM_RATE;
+        } else if (mac_flags & IEEE80211_TX_RC_VHT_MCS) {
+                mac_index &= RATE_VHT_MCS_RATE_CODE_MSK;
+                if (mac_index >= (IWL_RATE_9M_INDEX - IWL_FIRST_OFDM_RATE))
+                        mac_index++;
         }
+
         /* Here we actually compare this rate to the latest LQ command */
         if ((mac_index < 0) ||
             (tbl_type.is_SGI != !!(mac_flags & IEEE80211_TX_RC_SHORT_GI)) ||
-            (tbl_type.is_ht40 != !!(mac_flags & IEEE80211_TX_RC_40_MHZ_WIDTH)) ||
+            (tbl_type.bw != rs_ch_width_from_mac_flags(mac_flags)) ||
             (tbl_type.ant_type != info->status.antenna) ||
             (!!(tx_rate & RATE_MCS_HT_MSK) !=
-                                !!(mac_flags & IEEE80211_TX_RC_MCS)) ||
+             !!(mac_flags & IEEE80211_TX_RC_MCS)) ||
+            (!!(tx_rate & RATE_MCS_VHT_MSK) !=
+             !!(mac_flags & IEEE80211_TX_RC_VHT_MCS)) ||
             (!!(tx_rate & RATE_HT_MCS_GF_MSK) !=
-                                !!(mac_flags & IEEE80211_TX_RC_GREEN_FIELD)) ||
+             !!(mac_flags & IEEE80211_TX_RC_GREEN_FIELD)) ||
             (rs_index != mac_index)) {
                 IWL_DEBUG_RATE(mvm,
                                "initial rate %d does not match %d (0x%x)\n",
@@ -947,7 +997,8 @@ static void rs_set_expected_tpt_table(struct iwl_lq_sta *lq_sta,
         s32 (*ht_tbl_pointer)[IWL_RATE_COUNT];
 
         /* Check for invalid LQ type */
-        if (WARN_ON_ONCE(!is_legacy(tbl->lq_type) && !is_Ht(tbl->lq_type))) {
+        if (WARN_ON_ONCE(!is_legacy(tbl->lq_type) && !is_ht(tbl->lq_type) &&
+                         !(is_vht(tbl->lq_type)))) {
                 tbl->expected_tpt = expected_tpt_legacy;
                 return;
         }
@@ -958,18 +1009,40 @@ static void rs_set_expected_tpt_table(struct iwl_lq_sta *lq_sta,
                 return;
         }
 
+        ht_tbl_pointer = expected_tpt_mimo2_20MHz;
         /* Choose among many HT tables depending on number of streams
-         * (SISO/MIMO2), channel width (20/40), SGI, and aggregation
+         * (SISO/MIMO2), channel width (20/40/80), SGI, and aggregation
          * status */
-        if (is_siso(tbl->lq_type) && !tbl->is_ht40)
-                ht_tbl_pointer = expected_tpt_siso20MHz;
-        else if (is_siso(tbl->lq_type))
-                ht_tbl_pointer = expected_tpt_siso40MHz;
-        else if (is_mimo2(tbl->lq_type) && !tbl->is_ht40)
-                ht_tbl_pointer = expected_tpt_mimo2_20MHz;
-        else {
-                WARN_ON_ONCE(!is_mimo2(tbl->lq_type));
-                ht_tbl_pointer = expected_tpt_mimo2_40MHz;
+        if (is_siso(tbl->lq_type)) {
+                switch (tbl->bw) {
+                case RATE_MCS_CHAN_WIDTH_20:
+                        ht_tbl_pointer = expected_tpt_siso_20MHz;
+                        break;
+                case RATE_MCS_CHAN_WIDTH_40:
+                        ht_tbl_pointer = expected_tpt_siso_40MHz;
+                        break;
+                case RATE_MCS_CHAN_WIDTH_80:
+                        ht_tbl_pointer = expected_tpt_siso_80MHz;
+                        break;
+                default:
+                        WARN_ON_ONCE(1);
+                }
+        } else if (is_mimo2(tbl->lq_type)) {
+                switch (tbl->bw) {
+                case RATE_MCS_CHAN_WIDTH_20:
+                        ht_tbl_pointer = expected_tpt_mimo2_20MHz;
+                        break;
+                case RATE_MCS_CHAN_WIDTH_40:
+                        ht_tbl_pointer = expected_tpt_mimo2_40MHz;
+                        break;
+                case RATE_MCS_CHAN_WIDTH_80:
+                        ht_tbl_pointer = expected_tpt_mimo2_80MHz;
+                        break;
+                default:
+                        WARN_ON_ONCE(1);
+                }
+        } else {
+                WARN_ON_ONCE(1);
         }
 
         if (!tbl->is_SGI && !lq_sta->is_agg)            /* Normal */
@@ -1084,9 +1157,47 @@ static s32 rs_get_best_rate(struct iwl_mvm *mvm,
         return new_rate;
 }
 
-static bool iwl_is_ht40_tx_allowed(struct ieee80211_sta *sta)
+/* Move to the next action and wrap around to the first action in case
+ * we're at the last action. Assumes actions start at 0.
+ */
+static inline void rs_move_next_action(struct iwl_scale_tbl_info *tbl,
+                                       u8 last_action)
+{
+        BUILD_BUG_ON(IWL_LEGACY_FIRST_ACTION != 0);
+        BUILD_BUG_ON(IWL_SISO_FIRST_ACTION != 0);
+        BUILD_BUG_ON(IWL_MIMO2_FIRST_ACTION != 0);
+
+        tbl->action = (tbl->action + 1) % (last_action + 1);
+}
+
+static void rs_set_bw_from_sta(struct iwl_scale_tbl_info *tbl,
+                               struct ieee80211_sta *sta)
+{
+        if (sta->bandwidth >= IEEE80211_STA_RX_BW_80)
+                tbl->bw = RATE_MCS_CHAN_WIDTH_80;
+        else if (sta->bandwidth >= IEEE80211_STA_RX_BW_40)
+                tbl->bw = RATE_MCS_CHAN_WIDTH_40;
+        else
+                tbl->bw = RATE_MCS_CHAN_WIDTH_20;
+}
+
+static bool rs_sgi_allowed(struct iwl_scale_tbl_info *tbl,
+                           struct ieee80211_sta *sta)
 {
-        return sta->bandwidth >= IEEE80211_STA_RX_BW_40;
+        struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
+        struct ieee80211_sta_vht_cap *vht_cap = &sta->vht_cap;
+
+        if (is_ht20(tbl) && (ht_cap->cap &
+                             IEEE80211_HT_CAP_SGI_20))
+                return true;
+        if (is_ht40(tbl) && (ht_cap->cap &
+                             IEEE80211_HT_CAP_SGI_40))
+                return true;
+        if (is_ht80(tbl) && (vht_cap->cap &
+                             IEEE80211_VHT_CAP_SHORT_GI_80))
+                return true;
+
+        return false;
 }
 
 /*
@@ -1099,7 +1210,6 @@ static int rs_switch_to_mimo2(struct iwl_mvm *mvm,
 {
         u16 rate_mask;
         s32 rate;
-        s8 is_green = lq_sta->is_green;
 
         if (!sta->ht_cap.ht_supported)
                 return -1;
@@ -1113,16 +1223,12 @@ static int rs_switch_to_mimo2(struct iwl_mvm *mvm,
 
         IWL_DEBUG_RATE(mvm, "LQ: try to switch to MIMO2\n");
 
-        tbl->lq_type = LQ_MIMO2;
+        tbl->lq_type = lq_sta->is_vht ? LQ_VHT_MIMO2 : LQ_HT_MIMO2;
         tbl->action = 0;
         tbl->max_search = IWL_MAX_SEARCH;
         rate_mask = lq_sta->active_mimo2_rate;
 
-        if (iwl_is_ht40_tx_allowed(sta))
-                tbl->is_ht40 = 1;
-        else
-                tbl->is_ht40 = 0;
-
+        rs_set_bw_from_sta(tbl, sta);
         rs_set_expected_tpt_table(lq_sta, tbl);
 
         rate = rs_get_best_rate(mvm, lq_sta, tbl, rate_mask, index);
@@ -1134,10 +1240,10 @@ static int rs_switch_to_mimo2(struct iwl_mvm *mvm,
                                rate, rate_mask);
                 return -1;
         }
-        tbl->current_rate = rate_n_flags_from_tbl(mvm, tbl, rate, is_green);
+        tbl->current_rate = rate_n_flags_from_tbl(mvm, tbl, rate);
 
-        IWL_DEBUG_RATE(mvm, "LQ: Switch to new mcs %X index is green %X\n",
-                       tbl->current_rate, is_green);
+        IWL_DEBUG_RATE(mvm, "LQ: Switch to new mcs %X index\n",
+                       tbl->current_rate);
         return 0;
 }
 
@@ -1150,7 +1256,6 @@ static int rs_switch_to_siso(struct iwl_mvm *mvm,
                              struct iwl_scale_tbl_info *tbl, int index)
 {
         u16 rate_mask;
-        u8 is_green = lq_sta->is_green;
         s32 rate;
 
         if (!sta->ht_cap.ht_supported)
@@ -1158,19 +1263,12 @@ static int rs_switch_to_siso(struct iwl_mvm *mvm,
 
         IWL_DEBUG_RATE(mvm, "LQ: try to switch to SISO\n");
 
-        tbl->lq_type = LQ_SISO;
+        tbl->lq_type = lq_sta->is_vht ? LQ_VHT_SISO : LQ_HT_SISO;
         tbl->action = 0;
         tbl->max_search = IWL_MAX_SEARCH;
         rate_mask = lq_sta->active_siso_rate;
 
-        if (iwl_is_ht40_tx_allowed(sta))
-                tbl->is_ht40 = 1;
-        else
-                tbl->is_ht40 = 0;
-
-        if (is_green)
-                tbl->is_SGI = 0; /*11n spec: no SGI in SISO+Greenfield*/
-
+        rs_set_bw_from_sta(tbl, sta);
         rs_set_expected_tpt_table(lq_sta, tbl);
         rate = rs_get_best_rate(mvm, lq_sta, tbl, rate_mask, index);
 
@@ -1181,9 +1279,9 @@ static int rs_switch_to_siso(struct iwl_mvm *mvm,
                                rate, rate_mask);
                 return -1;
         }
-        tbl->current_rate = rate_n_flags_from_tbl(mvm, tbl, rate, is_green);
-        IWL_DEBUG_RATE(mvm, "LQ: Switch to new mcs %X index is green %X\n",
-                       tbl->current_rate, is_green);
+        tbl->current_rate = rate_n_flags_from_tbl(mvm, tbl, rate);
+        IWL_DEBUG_RATE(mvm, "LQ: Switch to new mcs %X index\n",
+                       tbl->current_rate);
         return 0;
 }
 
@@ -1211,14 +1309,10 @@ static int rs_move_legacy_other(struct iwl_mvm *mvm,
         while (1) {
                 lq_sta->action_counter++;
                 switch (tbl->action) {
-                case IWL_LEGACY_SWITCH_ANTENNA1:
-                case IWL_LEGACY_SWITCH_ANTENNA2:
+                case IWL_LEGACY_SWITCH_ANTENNA:
                         IWL_DEBUG_RATE(mvm, "LQ: Legacy toggle Antenna\n");
 
-                        if ((tbl->action == IWL_LEGACY_SWITCH_ANTENNA1 &&
-                             tx_chains_num <= 1) ||
-                            (tbl->action == IWL_LEGACY_SWITCH_ANTENNA2 &&
-                             tx_chains_num <= 2))
+                        if (tx_chains_num <= 1)
                                 break;
 
                         /* Don't change antenna if success has been great */
@@ -1273,9 +1367,7 @@ static int rs_move_legacy_other(struct iwl_mvm *mvm,
                 default:
                         WARN_ON_ONCE(1);
                 }
-                tbl->action++;
-                if (tbl->action > IWL_LEGACY_SWITCH_MIMO2)
-                        tbl->action = IWL_LEGACY_SWITCH_ANTENNA1;
+                rs_move_next_action(tbl, IWL_LEGACY_LAST_ACTION);
 
                 if (tbl->action == start_action)
                         break;
@@ -1285,9 +1377,7 @@ static int rs_move_legacy_other(struct iwl_mvm *mvm,
 
 out:
         lq_sta->search_better_tbl = 1;
-        tbl->action++;
-        if (tbl->action > IWL_LEGACY_SWITCH_MIMO2)
-                tbl->action = IWL_LEGACY_SWITCH_ANTENNA1;
+        rs_move_next_action(tbl, IWL_LEGACY_LAST_ACTION);
         if (update_search_tbl_counter)
                 search_tbl->action = tbl->action;
         return 0;
@@ -1300,12 +1390,10 @@ static int rs_move_siso_to_other(struct iwl_mvm *mvm,
                                  struct iwl_lq_sta *lq_sta,
                                  struct ieee80211_sta *sta, int index)
 {
-        u8 is_green = lq_sta->is_green;
         struct iwl_scale_tbl_info *tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
         struct iwl_scale_tbl_info *search_tbl =
                                 &(lq_sta->lq_info[(1 - lq_sta->active_tbl)]);
         struct iwl_rate_scale_data *window = &(tbl->win[index]);
-        struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
         u32 sz = (sizeof(struct iwl_scale_tbl_info) -
                   (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
         u8 start_action;
@@ -1314,40 +1402,17 @@ static int rs_move_siso_to_other(struct iwl_mvm *mvm,
         u8 update_search_tbl_counter = 0;
         int ret;
 
-        switch (BT_MBOX_MSG(&mvm->last_bt_notif, 3, TRAFFIC_LOAD)) {
-        case IWL_BT_COEX_TRAFFIC_LOAD_NONE:
-                /* nothing */
-                break;
-        case IWL_BT_COEX_TRAFFIC_LOAD_LOW:
-                /* avoid antenna B unless MIMO */
-                if (tbl->action == IWL_SISO_SWITCH_ANTENNA2)
-                        tbl->action = IWL_SISO_SWITCH_MIMO2;
-                break;
-        case IWL_BT_COEX_TRAFFIC_LOAD_HIGH:
-        case IWL_BT_COEX_TRAFFIC_LOAD_CONTINUOUS:
-                /* avoid antenna B and MIMO */
-                valid_tx_ant =
-                        first_antenna(iwl_fw_valid_tx_ant(mvm->fw));
-                if (tbl->action != IWL_SISO_SWITCH_ANTENNA1)
-                        tbl->action = IWL_SISO_SWITCH_ANTENNA1;
-                break;
-        default:
-                IWL_ERR(mvm, "Invalid BT load %d",
-                        BT_MBOX_MSG(&mvm->last_bt_notif, 3, TRAFFIC_LOAD));
-                break;
-        }
+        if (tbl->action == IWL_SISO_SWITCH_MIMO2 &&
+            !iwl_mvm_bt_coex_is_mimo_allowed(mvm, sta))
+                tbl->action = IWL_SISO_SWITCH_ANTENNA;
 
         start_action = tbl->action;
         while (1) {
                 lq_sta->action_counter++;
                 switch (tbl->action) {
-                case IWL_SISO_SWITCH_ANTENNA1:
-                case IWL_SISO_SWITCH_ANTENNA2:
+                case IWL_SISO_SWITCH_ANTENNA:
                         IWL_DEBUG_RATE(mvm, "LQ: SISO toggle Antenna\n");
-                        if ((tbl->action == IWL_SISO_SWITCH_ANTENNA1 &&
-                             tx_chains_num <= 1) ||
-                            (tbl->action == IWL_SISO_SWITCH_ANTENNA2 &&
-                             tx_chains_num <= 2))
+                        if (tx_chains_num <= 1)
                                 break;
 
                         if (window->success_ratio >= IWL_RS_GOOD_RATIO &&
@@ -1380,23 +1445,12 @@ static int rs_move_siso_to_other(struct iwl_mvm *mvm,
                                 goto out;
                         break;
                 case IWL_SISO_SWITCH_GI:
-                        if (!tbl->is_ht40 && !(ht_cap->cap &
-                                                IEEE80211_HT_CAP_SGI_20))
-                                break;
-                        if (tbl->is_ht40 && !(ht_cap->cap &
-                                                IEEE80211_HT_CAP_SGI_40))
+                        if (!rs_sgi_allowed(tbl, sta))
                                 break;
 
                         IWL_DEBUG_RATE(mvm, "LQ: SISO toggle SGI/NGI\n");
 
                         memcpy(search_tbl, tbl, sz);
-                        if (is_green) {
-                                if (!tbl->is_SGI)
-                                        break;
-                                else
-                                        IWL_ERR(mvm,
-                                                "SGI was set in GF+SISO\n");
-                        }
                         search_tbl->is_SGI = !tbl->is_SGI;
                         rs_set_expected_tpt_table(lq_sta, search_tbl);
                         if (tbl->is_SGI) {
@@ -1405,16 +1459,13 @@ static int rs_move_siso_to_other(struct iwl_mvm *mvm,
                                         break;
                         }
                         search_tbl->current_rate =
-                                rate_n_flags_from_tbl(mvm, search_tbl,
-                                                      index, is_green);
+                                rate_n_flags_from_tbl(mvm, search_tbl, index);
                         update_search_tbl_counter = 1;
                         goto out;
                 default:
                         WARN_ON_ONCE(1);
                 }
-                tbl->action++;
-                if (tbl->action > IWL_SISO_SWITCH_GI)
-                        tbl->action = IWL_SISO_SWITCH_ANTENNA1;
+                rs_move_next_action(tbl, IWL_SISO_LAST_ACTION);
 
                 if (tbl->action == start_action)
                         break;
@@ -1424,9 +1475,7 @@ static int rs_move_siso_to_other(struct iwl_mvm *mvm,
 
  out:
         lq_sta->search_better_tbl = 1;
-        tbl->action++;
-        if (tbl->action > IWL_SISO_SWITCH_GI)
-                tbl->action = IWL_SISO_SWITCH_ANTENNA1;
+        rs_move_next_action(tbl, IWL_SISO_LAST_ACTION);
         if (update_search_tbl_counter)
                 search_tbl->action = tbl->action;
 
@@ -1440,63 +1489,20 @@ static int rs_move_mimo2_to_other(struct iwl_mvm *mvm,
                                  struct iwl_lq_sta *lq_sta,
                                  struct ieee80211_sta *sta, int index)
 {
-        s8 is_green = lq_sta->is_green;
         struct iwl_scale_tbl_info *tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
         struct iwl_scale_tbl_info *search_tbl =
                                 &(lq_sta->lq_info[(1 - lq_sta->active_tbl)]);
-        struct iwl_rate_scale_data *window = &(tbl->win[index]);
-        struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
         u32 sz = (sizeof(struct iwl_scale_tbl_info) -
                   (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
         u8 start_action;
         u8 valid_tx_ant = iwl_fw_valid_tx_ant(mvm->fw);
-        u8 tx_chains_num = num_of_ant(valid_tx_ant);
         u8 update_search_tbl_counter = 0;
         int ret;
 
-        switch (BT_MBOX_MSG(&mvm->last_bt_notif, 3, TRAFFIC_LOAD)) {
-        case IWL_BT_COEX_TRAFFIC_LOAD_NONE:
-                /* nothing */
-                break;
-        case IWL_BT_COEX_TRAFFIC_LOAD_HIGH:
-        case IWL_BT_COEX_TRAFFIC_LOAD_CONTINUOUS:
-                /* avoid antenna B and MIMO */
-                if (tbl->action != IWL_MIMO2_SWITCH_SISO_A)
-                        tbl->action = IWL_MIMO2_SWITCH_SISO_A;
-                break;
-        case IWL_BT_COEX_TRAFFIC_LOAD_LOW:
-                /* avoid antenna B unless MIMO */
-                if (tbl->action == IWL_MIMO2_SWITCH_SISO_B)
-                        tbl->action = IWL_MIMO2_SWITCH_SISO_A;
-                break;
-        default:
-                IWL_ERR(mvm, "Invalid BT load %d",
-                        BT_MBOX_MSG(&mvm->last_bt_notif, 3, TRAFFIC_LOAD));
-                break;
-        }
-
         start_action = tbl->action;
         while (1) {
                 lq_sta->action_counter++;
                 switch (tbl->action) {
-                case IWL_MIMO2_SWITCH_ANTENNA1:
-                case IWL_MIMO2_SWITCH_ANTENNA2:
-                        IWL_DEBUG_RATE(mvm, "LQ: MIMO2 toggle Antennas\n");
-
-                        if (tx_chains_num <= 2)
-                                break;
-
-                        if (window->success_ratio >= IWL_RS_GOOD_RATIO)
-                                break;
-
-                        memcpy(search_tbl, tbl, sz);
-                        if (rs_toggle_antenna(valid_tx_ant,
-                                              &search_tbl->current_rate,
-                                              search_tbl)) {
-                                update_search_tbl_counter = 1;
-                                goto out;
-                        }
-                        break;
                 case IWL_MIMO2_SWITCH_SISO_A:
                 case IWL_MIMO2_SWITCH_SISO_B:
                         IWL_DEBUG_RATE(mvm, "LQ: MIMO2 switch to SISO\n");
@@ -1521,11 +1527,7 @@ static int rs_move_mimo2_to_other(struct iwl_mvm *mvm,
                         break;
 
                 case IWL_MIMO2_SWITCH_GI:
-                        if (!tbl->is_ht40 && !(ht_cap->cap &
-                                                IEEE80211_HT_CAP_SGI_20))
-                                break;
-                        if (tbl->is_ht40 && !(ht_cap->cap &
-                                                IEEE80211_HT_CAP_SGI_40))
+                        if (!rs_sgi_allowed(tbl, sta))
                                 break;
 
                         IWL_DEBUG_RATE(mvm, "LQ: MIMO2 toggle SGI/NGI\n");
@@ -1546,16 +1548,13 @@ static int rs_move_mimo2_to_other(struct iwl_mvm *mvm,
                                         break;
                         }
                         search_tbl->current_rate =
-                                rate_n_flags_from_tbl(mvm, search_tbl,
-                                                      index, is_green);
+                                rate_n_flags_from_tbl(mvm, search_tbl, index);
                         update_search_tbl_counter = 1;
                         goto out;
                 default:
                         WARN_ON_ONCE(1);
                 }
-                tbl->action++;
-                if (tbl->action > IWL_MIMO2_SWITCH_GI)
-                        tbl->action = IWL_MIMO2_SWITCH_ANTENNA1;
+                rs_move_next_action(tbl, IWL_MIMO2_LAST_ACTION);
 
                 if (tbl->action == start_action)
                         break;
@@ -1564,9 +1563,7 @@ static int rs_move_mimo2_to_other(struct iwl_mvm *mvm,
         return 0;
  out:
         lq_sta->search_better_tbl = 1;
-        tbl->action++;
-        if (tbl->action > IWL_MIMO2_SWITCH_GI)
-                tbl->action = IWL_MIMO2_SWITCH_ANTENNA1;
+        rs_move_next_action(tbl, IWL_MIMO2_LAST_ACTION);
         if (update_search_tbl_counter)
                 search_tbl->action = tbl->action;
 
@@ -1660,15 +1657,16 @@ static void rs_stay_in_table(struct iwl_lq_sta *lq_sta, bool force_search)
  * setup rate table in uCode
  */
 static void rs_update_rate_tbl(struct iwl_mvm *mvm,
+                               struct ieee80211_sta *sta,
                                struct iwl_lq_sta *lq_sta,
                                struct iwl_scale_tbl_info *tbl,
-                               int index, u8 is_green)
+                               int index)
 {
         u32 rate;
 
         /* Update uCode's rate table. */
-        rate = rate_n_flags_from_tbl(mvm, tbl, index, is_green);
-        rs_fill_link_cmd(mvm, lq_sta, rate);
+        rate = rate_n_flags_from_tbl(mvm, tbl, index);
+        rs_fill_link_cmd(mvm, sta, lq_sta, rate);
         iwl_mvm_send_lq_cmd(mvm, &lq_sta->lq, CMD_ASYNC, false);
 }
 
@@ -1712,7 +1710,6 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
         u8 update_lq = 0;
         struct iwl_scale_tbl_info *tbl, *tbl1;
         u16 rate_scale_index_msk = 0;
-        u8 is_green = 0;
         u8 active_tbl = 0;
         u8 done_search = 0;
         u16 high_low;
@@ -1754,11 +1751,6 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
                 active_tbl = 1 - lq_sta->active_tbl;
 
         tbl = &(lq_sta->lq_info[active_tbl]);
-        if (is_legacy(tbl->lq_type))
-                lq_sta->is_green = 0;
-        else
-                lq_sta->is_green = rs_use_green(sta);
-        is_green = lq_sta->is_green;
 
         /* current tx rate */
         index = lq_sta->last_txrate_idx;
@@ -1797,7 +1789,7 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
                         tbl = &(lq_sta->lq_info[lq_sta->active_tbl]);
                         /* get "active" rate info */
                         index = iwl_hwrate_to_plcp_idx(tbl->current_rate);
-                        rs_update_rate_tbl(mvm, lq_sta, tbl, index, is_green);
+                        rs_update_rate_tbl(mvm, sta, lq_sta, tbl, index);
                 }
                 return;
         }
@@ -1978,24 +1970,24 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
              (current_tpt > (100 * tbl->expected_tpt[low]))))
                 scale_action = 0;
 
-        if ((BT_MBOX_MSG(&mvm->last_bt_notif, 3, TRAFFIC_LOAD) >=
+        if ((le32_to_cpu(mvm->last_bt_notif.bt_activity_grading) >=
              IWL_BT_COEX_TRAFFIC_LOAD_HIGH) && (is_mimo(tbl->lq_type))) {
                 if (lq_sta->last_bt_traffic >
-                    BT_MBOX_MSG(&mvm->last_bt_notif, 3, TRAFFIC_LOAD)) {
+                    le32_to_cpu(mvm->last_bt_notif.bt_activity_grading)) {
                         /*
                          * don't set scale_action, don't want to scale up if
                          * the rate scale doesn't otherwise think that is a
                          * good idea.
                          */
                 } else if (lq_sta->last_bt_traffic <=
-                           BT_MBOX_MSG(&mvm->last_bt_notif, 3, TRAFFIC_LOAD)) {
+                           le32_to_cpu(mvm->last_bt_notif.bt_activity_grading)) {
                         scale_action = -1;
                 }
         }
         lq_sta->last_bt_traffic =
-                BT_MBOX_MSG(&mvm->last_bt_notif, 3, TRAFFIC_LOAD);
+                le32_to_cpu(mvm->last_bt_notif.bt_activity_grading);
 
-        if ((BT_MBOX_MSG(&mvm->last_bt_notif, 3, TRAFFIC_LOAD) >=
+        if ((le32_to_cpu(mvm->last_bt_notif.bt_activity_grading) >=
              IWL_BT_COEX_TRAFFIC_LOAD_HIGH) && is_mimo(tbl->lq_type)) {
                 /* search for a new modulation */
                 rs_stay_in_table(lq_sta, true);
@@ -2032,7 +2024,7 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
 lq_update:
         /* Replace uCode's rate table for the destination station. */
         if (update_lq)
-                rs_update_rate_tbl(mvm, lq_sta, tbl, index, is_green);
+                rs_update_rate_tbl(mvm, sta, lq_sta, tbl, index);
 
         rs_stay_in_table(lq_sta, false);
 
@@ -2071,7 +2063,7 @@ lq_update:
                         IWL_DEBUG_RATE(mvm,
                                        "Switch current  mcs: %X index: %d\n",
                                        tbl->current_rate, index);
-                        rs_fill_link_cmd(mvm, lq_sta, tbl->current_rate);
+                        rs_fill_link_cmd(mvm, sta, lq_sta, tbl->current_rate);
                         iwl_mvm_send_lq_cmd(mvm, &lq_sta->lq, CMD_ASYNC, false);
                 } else {
                         done_search = 1;
@@ -2113,7 +2105,7 @@ lq_update:
         }
 
 out:
-        tbl->current_rate = rate_n_flags_from_tbl(mvm, tbl, index, is_green);
+        tbl->current_rate = rate_n_flags_from_tbl(mvm, tbl, index);
         lq_sta->last_txrate_idx = index;
 }
 
@@ -2140,7 +2132,6 @@ static void rs_initialize_lq(struct iwl_mvm *mvm,
         int rate_idx;
         int i;
         u32 rate;
-        u8 use_green = rs_use_green(sta);
         u8 active_tbl = 0;
         u8 valid_tx_ant;
 
@@ -2172,10 +2163,10 @@ static void rs_initialize_lq(struct iwl_mvm *mvm,
         if (!rs_is_valid_ant(valid_tx_ant, tbl->ant_type))
                 rs_toggle_antenna(valid_tx_ant, &rate, tbl);
 
-        rate = rate_n_flags_from_tbl(mvm, tbl, rate_idx, use_green);
+        rate = rate_n_flags_from_tbl(mvm, tbl, rate_idx);
         tbl->current_rate = rate;
         rs_set_expected_tpt_table(lq_sta, tbl);
-        rs_fill_link_cmd(NULL, lq_sta, rate);
+        rs_fill_link_cmd(NULL, NULL, lq_sta, rate);
         /* TODO restore station should remember the lq cmd */
         iwl_mvm_send_lq_cmd(mvm, &lq_sta->lq, CMD_SYNC, true);
 }
@@ -2190,7 +2181,6 @@ static void rs_get_rate(void *mvm_r, struct ieee80211_sta *sta, void *mvm_sta,
         struct iwl_mvm *mvm __maybe_unused = IWL_OP_MODE_GET_MVM(op_mode);
         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         struct iwl_lq_sta *lq_sta = mvm_sta;
-        int rate_idx;
 
         IWL_DEBUG_RATE_LIMIT(mvm, "rate scale calculate new rate for skb\n");
 
@@ -2215,36 +2205,9 @@ static void rs_get_rate(void *mvm_r, struct ieee80211_sta *sta, void *mvm_sta,
         if (rate_control_send_low(sta, mvm_sta, txrc))
                 return;
 
-        rate_idx  = lq_sta->last_txrate_idx;
-
-        if (lq_sta->last_rate_n_flags & RATE_MCS_HT_MSK) {
-                rate_idx -= IWL_FIRST_OFDM_RATE;
-                /* 6M and 9M shared same MCS index */
-                rate_idx = (rate_idx > 0) ? (rate_idx - 1) : 0;
-                WARN_ON_ONCE(rs_extract_rate(lq_sta->last_rate_n_flags) >=
-                             IWL_RATE_MIMO3_6M_PLCP);
-                if (rs_extract_rate(lq_sta->last_rate_n_flags) >=
-                    IWL_RATE_MIMO2_6M_PLCP)
-                        rate_idx = rate_idx + MCS_INDEX_PER_STREAM;
-                info->control.rates[0].flags = IEEE80211_TX_RC_MCS;
-                if (lq_sta->last_rate_n_flags & RATE_MCS_SGI_MSK)
-                        info->control.rates[0].flags |= IEEE80211_TX_RC_SHORT_GI;
-                if (lq_sta->last_rate_n_flags & RATE_MCS_CHAN_WIDTH_40) /* TODO */
-                        info->control.rates[0].flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
-                if (lq_sta->last_rate_n_flags & RATE_HT_MCS_GF_MSK)
-                        info->control.rates[0].flags |= IEEE80211_TX_RC_GREEN_FIELD;
-        } else {
-                /* Check for invalid rates */
-                if ((rate_idx < 0) || (rate_idx >= IWL_RATE_COUNT_LEGACY) ||
-                    ((sband->band == IEEE80211_BAND_5GHZ) &&
-                     (rate_idx < IWL_FIRST_OFDM_RATE)))
-                        rate_idx = rate_lowest_index(sband, sta);
-                /* On valid 5 GHz rate, adjust index */
-                else if (sband->band == IEEE80211_BAND_5GHZ)
-                        rate_idx -= IWL_FIRST_OFDM_RATE;
-                info->control.rates[0].flags = 0;
-        }
-        info->control.rates[0].idx = rate_idx;
+        iwl_mvm_hwrate_to_tx_rate(lq_sta->last_rate_n_flags,
+                                  info->band, &info->control.rates[0]);
+
         info->control.rates[0].count = 1;
 }
 
@@ -2261,6 +2224,24 @@ static void *rs_alloc_sta(void *mvm_rate, struct ieee80211_sta *sta,
         return &sta_priv->lq_sta;
 }
 
+static int rs_vht_highest_rx_mcs_index(struct ieee80211_sta_vht_cap *vht_cap,
+                                       int nss)
+{
+        u16 rx_mcs = le16_to_cpu(vht_cap->vht_mcs.rx_mcs_map) &
+                (0x3 << (2 * (nss - 1)));
+        rx_mcs >>= (2 * (nss - 1));
+
+        if (rx_mcs == IEEE80211_VHT_MCS_SUPPORT_0_7)
+                return IWL_RATE_MCS_7_INDEX;
+        else if (rx_mcs == IEEE80211_VHT_MCS_SUPPORT_0_8)
+                return IWL_RATE_MCS_8_INDEX;
+        else if (rx_mcs == IEEE80211_VHT_MCS_SUPPORT_0_9)
+                return IWL_RATE_MCS_9_INDEX;
+
+        WARN_ON_ONCE(rx_mcs != IEEE80211_VHT_MCS_NOT_SUPPORTED);
+        return -1;
+}
+
 /*
  * Called after adding a new station to initialize rate scaling
  */
@@ -2270,6 +2251,7 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
         int i, j;
         struct ieee80211_hw *hw = mvm->hw;
         struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
+        struct ieee80211_sta_vht_cap *vht_cap = &sta->vht_cap;
         struct iwl_mvm_sta *sta_priv;
         struct iwl_lq_sta *lq_sta;
         struct ieee80211_supported_band *sband;
@@ -2298,7 +2280,6 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 
         lq_sta->max_rate_idx = -1;
         lq_sta->missed_rate_counter = IWL_MISSED_RATE_MAX;
-        lq_sta->is_green = rs_use_green(sta);
         lq_sta->band = sband->band;
         /*
          * active legacy rates as per supported rates bitmap
@@ -2308,25 +2289,54 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
         for_each_set_bit(i, &supp, BITS_PER_LONG)
                 lq_sta->active_legacy_rate |= BIT(sband->bitrates[i].hw_value);
 
-        /*
-         * active_siso_rate mask includes 9 MBits (bit 5), and CCK (bits 0-3),
-         * supp_rates[] does not; shift to convert format, force 9 MBits off.
-         */
-        lq_sta->active_siso_rate = ht_cap->mcs.rx_mask[0] << 1;
-        lq_sta->active_siso_rate |= ht_cap->mcs.rx_mask[0] & 0x1;
-        lq_sta->active_siso_rate &= ~((u16)0x2);
-        lq_sta->active_siso_rate <<= IWL_FIRST_OFDM_RATE;
+        /* TODO: should probably account for rx_highest for both HT/VHT */
+        if (!vht_cap || !vht_cap->vht_supported) {
+                /* active_siso_rate mask includes 9 MBits (bit 5),
+                 * and CCK (bits 0-3), supp_rates[] does not;
+                 * shift to convert format, force 9 MBits off.
+                 */
+                lq_sta->active_siso_rate = ht_cap->mcs.rx_mask[0] << 1;
+                lq_sta->active_siso_rate |= ht_cap->mcs.rx_mask[0] & 0x1;
+                lq_sta->active_siso_rate &= ~((u16)0x2);
+                lq_sta->active_siso_rate <<= IWL_FIRST_OFDM_RATE;
+
+                /* Same here */
+                lq_sta->active_mimo2_rate = ht_cap->mcs.rx_mask[1] << 1;
+                lq_sta->active_mimo2_rate |= ht_cap->mcs.rx_mask[1] & 0x1;
+                lq_sta->active_mimo2_rate &= ~((u16)0x2);
+                lq_sta->active_mimo2_rate <<= IWL_FIRST_OFDM_RATE;
+
+                lq_sta->is_vht = false;
+        } else {
+                int highest_mcs = rs_vht_highest_rx_mcs_index(vht_cap, 1);
+                if (highest_mcs >= IWL_RATE_MCS_0_INDEX) {
+                        for (i = IWL_RATE_MCS_0_INDEX; i <= highest_mcs; i++) {
+                                if (i == IWL_RATE_9M_INDEX)
+                                        continue;
+
+                                lq_sta->active_siso_rate |= BIT(i);
+                        }
+                }
+
+                highest_mcs = rs_vht_highest_rx_mcs_index(vht_cap, 2);
+                if (highest_mcs >= IWL_RATE_MCS_0_INDEX) {
+                        for (i = IWL_RATE_MCS_0_INDEX; i <= highest_mcs; i++) {
+                                if (i == IWL_RATE_9M_INDEX)
+                                        continue;
 
-        /* Same here */
-        lq_sta->active_mimo2_rate = ht_cap->mcs.rx_mask[1] << 1;
-        lq_sta->active_mimo2_rate |= ht_cap->mcs.rx_mask[1] & 0x1;
-        lq_sta->active_mimo2_rate &= ~((u16)0x2);
-        lq_sta->active_mimo2_rate <<= IWL_FIRST_OFDM_RATE;
+                                lq_sta->active_mimo2_rate |= BIT(i);
+                        }
+                }
+
+                /* TODO: avoid MCS9 in 20Mhz which isn't valid for 11ac */
+                lq_sta->is_vht = true;
+        }
 
         IWL_DEBUG_RATE(mvm,
-                       "SISO-RATE=%X MIMO2-RATE=%X\n",
+                       "SISO-RATE=%X MIMO2-RATE=%X VHT=%d\n",
                        lq_sta->active_siso_rate,
-                       lq_sta->active_mimo2_rate);
+                       lq_sta->active_mimo2_rate,
+                       lq_sta->is_vht);
 
         /* These values will be overridden later */
         lq_sta->lq.single_stream_ant_msk =
@@ -2358,6 +2368,7 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 }
 
 static void rs_fill_link_cmd(struct iwl_mvm *mvm,
+                             struct ieee80211_sta *sta,
                              struct iwl_lq_sta *lq_sta, u32 new_rate)
 {
         struct iwl_scale_tbl_info tbl_type;
@@ -2429,7 +2440,6 @@ static void rs_fill_link_cmd(struct iwl_mvm *mvm,
                 rs_get_tbl_info_from_mcs(new_rate, lq_sta->band, &tbl_type,
                                          &rate_idx);
 
-
                 /* Indicate to uCode which entries might be MIMO.
                  * If initial rate was MIMO, this will finally end up
                  * as (IWL_HT_NUMBER_TRY * 2), after 2nd pass, otherwise 0. */
@@ -2455,7 +2465,9 @@ static void rs_fill_link_cmd(struct iwl_mvm *mvm,
                 }
 
                 /* Don't allow HT rates after next pass.
-                 * rs_get_lower_rate() will change type to LQ_A or LQ_G. */
+                 * rs_get_lower_rate() will change type to LQ_LEGACY_A
+                 * or LQ_LEGACY_G.
+                 */
                 use_ht_possible = 0;
 
                 /* Override next rate if needed for debug purposes */
@@ -2474,12 +2486,9 @@ static void rs_fill_link_cmd(struct iwl_mvm *mvm,
         lq_cmd->agg_time_limit =
                 cpu_to_le16(LINK_QUAL_AGG_TIME_LIMIT_DEF);
 
-        /*
-         * overwrite if needed, pass aggregation time limit
-         * to uCode in uSec - This is racy - but heh, at least it helps...
-         */
-        if (mvm && BT_MBOX_MSG(&mvm->last_bt_notif, 3, TRAFFIC_LOAD) >= 2)
-                lq_cmd->agg_time_limit = cpu_to_le16(1200);
+        if (sta)
+                lq_cmd->agg_time_limit =
+                        cpu_to_le16(iwl_mvm_bt_coex_agg_time_limit(mvm, sta));
 }
 
 static void *rs_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
@@ -2586,16 +2595,18 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
             (iwl_fw_valid_tx_ant(mvm->fw) & ANT_B) ? "ANT_B," : "",
             (iwl_fw_valid_tx_ant(mvm->fw) & ANT_C) ? "ANT_C" : "");
         desc += sprintf(buff+desc, "lq type %s\n",
-           (is_legacy(tbl->lq_type)) ? "legacy" : "HT");
-        if (is_Ht(tbl->lq_type)) {
+                        (is_legacy(tbl->lq_type)) ? "legacy" :
+                        is_vht(tbl->lq_type) ? "VHT" : "HT");
+        if (is_ht(tbl->lq_type)) {
                 desc += sprintf(buff+desc, " %s",
                    (is_siso(tbl->lq_type)) ? "SISO" : "MIMO2");
                    desc += sprintf(buff+desc, " %s",
-                   (tbl->is_ht40) ? "40MHz" : "20MHz");
-                   desc += sprintf(buff+desc, " %s %s %s\n",
+                                   (is_ht20(tbl)) ? "20MHz" :
+                                   (is_ht40(tbl)) ? "40MHz" :
+                                   (is_ht80(tbl)) ? "80Mhz" : "BAD BW");
+                   desc += sprintf(buff+desc, " %s %s\n",
                                    (tbl->is_SGI) ? "SGI" : "",
-                   (lq_sta->is_green) ? "GF enabled" : "",
-                   (lq_sta->is_agg) ? "AGG on" : "");
+                                   (lq_sta->is_agg) ? "AGG on" : "");
         }
         desc += sprintf(buff+desc, "last tx rate=0x%X\n",
                         lq_sta->last_rate_n_flags);
@@ -2653,7 +2664,7 @@ static ssize_t rs_sta_dbgfs_stats_table_read(struct file *file,
         int desc = 0;
         int i, j;
         ssize_t ret;
-
+        struct iwl_scale_tbl_info *tbl;
         struct iwl_lq_sta *lq_sta = file->private_data;
 
         buff = kmalloc(1024, GFP_KERNEL);
@@ -2661,21 +2672,23 @@ static ssize_t rs_sta_dbgfs_stats_table_read(struct file *file,
                 return -ENOMEM;
 
         for (i = 0; i < LQ_SIZE; i++) {
+                tbl = &(lq_sta->lq_info[i]);
                 desc += sprintf(buff+desc,
-                                "%s type=%d SGI=%d HT40=%d DUP=0 GF=%d\n"
+                                "%s type=%d SGI=%d BW=%s DUP=0\n"
                                 "rate=0x%X\n",
                                 lq_sta->active_tbl == i ? "*" : "x",
-                                lq_sta->lq_info[i].lq_type,
-                                lq_sta->lq_info[i].is_SGI,
-                                lq_sta->lq_info[i].is_ht40,
-                                lq_sta->is_green,
-                                lq_sta->lq_info[i].current_rate);
+                                tbl->lq_type,
+                                tbl->is_SGI,
+                                is_ht20(tbl) ? "20Mhz" :
+                                is_ht40(tbl) ? "40Mhz" :
+                                is_ht80(tbl) ? "80Mhz" : "ERR",
+                                tbl->current_rate);
                 for (j = 0; j < IWL_RATE_COUNT; j++) {
                         desc += sprintf(buff+desc,
                                 "counter=%d success=%d %%=%d\n",
-                                lq_sta->lq_info[i].win[j].counter,
-                                lq_sta->lq_info[i].win[j].success_counter,
-                                lq_sta->lq_info[i].win[j].success_ratio);
+                                tbl->win[j].counter,
+                                tbl->win[j].success_counter,
+                                tbl->win[j].success_ratio);
                 }
         }
         ret = simple_read_from_buffer(user_buf, count, ppos, buff, desc);
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.h b/drivers/net/wireless/iwlwifi/mvm/rs.h
index 465d40ee176f..5d5344f7070b 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.h
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.h
@@ -35,9 +35,11 @@
 #include "iwl-trans.h"
 
 struct iwl_rs_rate_info {
-        u8 plcp;        /* uCode API:  IWL_RATE_6M_PLCP, etc. */
-        u8 plcp_siso;   /* uCode API:  IWL_RATE_SISO_6M_PLCP, etc. */
-        u8 plcp_mimo2;  /* uCode API:  IWL_RATE_MIMO2_6M_PLCP, etc. */
+        u8 plcp;          /* uCode API:  IWL_RATE_6M_PLCP, etc. */
+        u8 plcp_ht_siso;  /* uCode API:  IWL_RATE_SISO_6M_PLCP, etc. */
+        u8 plcp_ht_mimo2; /* uCode API:  IWL_RATE_MIMO2_6M_PLCP, etc. */
+        u8 plcp_vht_siso;
+        u8 plcp_vht_mimo2;
         u8 prev_rs;      /* previous rate used in rs algo */
         u8 next_rs;      /* next rate used in rs algo */
 };
@@ -83,35 +85,52 @@ enum {
 #define IWL_RATE_11M_MASK  (1 << IWL_RATE_11M_INDEX)
 
 
-/* uCode API values for OFDM high-throughput (HT) bit rates */
+/* uCode API values for HT/VHT bit rates */
 enum {
-        IWL_RATE_SISO_6M_PLCP = 0,
-        IWL_RATE_SISO_12M_PLCP = 1,
-        IWL_RATE_SISO_18M_PLCP = 2,
-        IWL_RATE_SISO_24M_PLCP = 3,
-        IWL_RATE_SISO_36M_PLCP = 4,
-        IWL_RATE_SISO_48M_PLCP = 5,
-        IWL_RATE_SISO_54M_PLCP = 6,
-        IWL_RATE_SISO_60M_PLCP = 7,
-        IWL_RATE_MIMO2_6M_PLCP  = 0x8,
-        IWL_RATE_MIMO2_12M_PLCP = 0x9,
-        IWL_RATE_MIMO2_18M_PLCP = 0xa,
-        IWL_RATE_MIMO2_24M_PLCP = 0xb,
-        IWL_RATE_MIMO2_36M_PLCP = 0xc,
-        IWL_RATE_MIMO2_48M_PLCP = 0xd,
-        IWL_RATE_MIMO2_54M_PLCP = 0xe,
-        IWL_RATE_MIMO2_60M_PLCP = 0xf,
-        IWL_RATE_MIMO3_6M_PLCP  = 0x10,
-        IWL_RATE_MIMO3_12M_PLCP = 0x11,
-        IWL_RATE_MIMO3_18M_PLCP = 0x12,
-        IWL_RATE_MIMO3_24M_PLCP = 0x13,
-        IWL_RATE_MIMO3_36M_PLCP = 0x14,
-        IWL_RATE_MIMO3_48M_PLCP = 0x15,
-        IWL_RATE_MIMO3_54M_PLCP = 0x16,
-        IWL_RATE_MIMO3_60M_PLCP = 0x17,
-        IWL_RATE_SISO_INVM_PLCP,
-        IWL_RATE_MIMO2_INVM_PLCP = IWL_RATE_SISO_INVM_PLCP,
-        IWL_RATE_MIMO3_INVM_PLCP = IWL_RATE_SISO_INVM_PLCP,
+        IWL_RATE_HT_SISO_MCS_0_PLCP = 0,
+        IWL_RATE_HT_SISO_MCS_1_PLCP = 1,
+        IWL_RATE_HT_SISO_MCS_2_PLCP = 2,
+        IWL_RATE_HT_SISO_MCS_3_PLCP = 3,
+        IWL_RATE_HT_SISO_MCS_4_PLCP = 4,
+        IWL_RATE_HT_SISO_MCS_5_PLCP = 5,
+        IWL_RATE_HT_SISO_MCS_6_PLCP = 6,
+        IWL_RATE_HT_SISO_MCS_7_PLCP = 7,
+        IWL_RATE_HT_MIMO2_MCS_0_PLCP = 0x8,
+        IWL_RATE_HT_MIMO2_MCS_1_PLCP = 0x9,
+        IWL_RATE_HT_MIMO2_MCS_2_PLCP = 0xA,
+        IWL_RATE_HT_MIMO2_MCS_3_PLCP = 0xB,
+        IWL_RATE_HT_MIMO2_MCS_4_PLCP = 0xC,
+        IWL_RATE_HT_MIMO2_MCS_5_PLCP = 0xD,
+        IWL_RATE_HT_MIMO2_MCS_6_PLCP = 0xE,
+        IWL_RATE_HT_MIMO2_MCS_7_PLCP = 0xF,
+        IWL_RATE_VHT_SISO_MCS_0_PLCP = 0,
+        IWL_RATE_VHT_SISO_MCS_1_PLCP = 1,
+        IWL_RATE_VHT_SISO_MCS_2_PLCP = 2,
+        IWL_RATE_VHT_SISO_MCS_3_PLCP = 3,
+        IWL_RATE_VHT_SISO_MCS_4_PLCP = 4,
+        IWL_RATE_VHT_SISO_MCS_5_PLCP = 5,
+        IWL_RATE_VHT_SISO_MCS_6_PLCP = 6,
+        IWL_RATE_VHT_SISO_MCS_7_PLCP = 7,
+        IWL_RATE_VHT_SISO_MCS_8_PLCP = 8,
+        IWL_RATE_VHT_SISO_MCS_9_PLCP = 9,
+        IWL_RATE_VHT_MIMO2_MCS_0_PLCP = 0x10,
+        IWL_RATE_VHT_MIMO2_MCS_1_PLCP = 0x11,
+        IWL_RATE_VHT_MIMO2_MCS_2_PLCP = 0x12,
+        IWL_RATE_VHT_MIMO2_MCS_3_PLCP = 0x13,
+        IWL_RATE_VHT_MIMO2_MCS_4_PLCP = 0x14,
+        IWL_RATE_VHT_MIMO2_MCS_5_PLCP = 0x15,
+        IWL_RATE_VHT_MIMO2_MCS_6_PLCP = 0x16,
+        IWL_RATE_VHT_MIMO2_MCS_7_PLCP = 0x17,
+        IWL_RATE_VHT_MIMO2_MCS_8_PLCP = 0x18,
+        IWL_RATE_VHT_MIMO2_MCS_9_PLCP = 0x19,
+        IWL_RATE_HT_SISO_MCS_INV_PLCP,
+        IWL_RATE_HT_MIMO2_MCS_INV_PLCP = IWL_RATE_HT_SISO_MCS_INV_PLCP,
+        IWL_RATE_VHT_SISO_MCS_INV_PLCP = IWL_RATE_HT_SISO_MCS_INV_PLCP,
+        IWL_RATE_VHT_MIMO2_MCS_INV_PLCP = IWL_RATE_HT_SISO_MCS_INV_PLCP,
+        IWL_RATE_HT_SISO_MCS_8_PLCP = IWL_RATE_HT_SISO_MCS_INV_PLCP,
+        IWL_RATE_HT_SISO_MCS_9_PLCP = IWL_RATE_HT_SISO_MCS_INV_PLCP,
+        IWL_RATE_HT_MIMO2_MCS_8_PLCP = IWL_RATE_HT_SISO_MCS_INV_PLCP,
+        IWL_RATE_HT_MIMO2_MCS_9_PLCP = IWL_RATE_HT_SISO_MCS_INV_PLCP,
 };
 
 #define IWL_RATES_MASK ((1 << IWL_RATE_COUNT) - 1)
@@ -139,25 +158,33 @@ enum {
 #define IWL_RATE_DECREASE_TH            1920    /*  15% */
 
 /* possible actions when in legacy mode */
-#define IWL_LEGACY_SWITCH_ANTENNA1      0
-#define IWL_LEGACY_SWITCH_ANTENNA2      1
-#define IWL_LEGACY_SWITCH_SISO          2
-#define IWL_LEGACY_SWITCH_MIMO2         3
+enum {
+        IWL_LEGACY_SWITCH_ANTENNA,
+        IWL_LEGACY_SWITCH_SISO,
+        IWL_LEGACY_SWITCH_MIMO2,
+        IWL_LEGACY_FIRST_ACTION = IWL_LEGACY_SWITCH_ANTENNA,
+        IWL_LEGACY_LAST_ACTION = IWL_LEGACY_SWITCH_MIMO2,
+};
 
 /* possible actions when in siso mode */
-#define IWL_SISO_SWITCH_ANTENNA1        0
-#define IWL_SISO_SWITCH_ANTENNA2        1
-#define IWL_SISO_SWITCH_MIMO2           2
-#define IWL_SISO_SWITCH_GI              3
+enum {
+        IWL_SISO_SWITCH_ANTENNA,
+        IWL_SISO_SWITCH_MIMO2,
+        IWL_SISO_SWITCH_GI,
+        IWL_SISO_FIRST_ACTION = IWL_SISO_SWITCH_ANTENNA,
+        IWL_SISO_LAST_ACTION = IWL_SISO_SWITCH_GI,
+};
 
 /* possible actions when in mimo mode */
-#define IWL_MIMO2_SWITCH_ANTENNA1       0
-#define IWL_MIMO2_SWITCH_ANTENNA2       1
-#define IWL_MIMO2_SWITCH_SISO_A         2
-#define IWL_MIMO2_SWITCH_SISO_B         3
-#define IWL_MIMO2_SWITCH_GI             4
+enum {
+        IWL_MIMO2_SWITCH_SISO_A,
+        IWL_MIMO2_SWITCH_SISO_B,
+        IWL_MIMO2_SWITCH_GI,
+        IWL_MIMO2_FIRST_ACTION = IWL_MIMO2_SWITCH_SISO_A,
+        IWL_MIMO2_LAST_ACTION = IWL_MIMO2_SWITCH_GI,
+};
 
-#define IWL_MAX_SEARCH IWL_MIMO2_SWITCH_GI
+#define IWL_MAX_SEARCH IWL_MIMO2_LAST_ACTION
 
 #define IWL_ACTION_LIMIT                3       /* # possible actions */
 
@@ -188,20 +215,31 @@ enum {
 
 enum iwl_table_type {
         LQ_NONE,
-        LQ_G,           /* legacy types */
-        LQ_A,
-        LQ_SISO,        /* high-throughput types */
-        LQ_MIMO2,
+        LQ_LEGACY_G,    /* legacy types */
+        LQ_LEGACY_A,
+        LQ_HT_SISO,     /* HT types */
+        LQ_HT_MIMO2,
+        LQ_VHT_SISO,    /* VHT types */
+        LQ_VHT_MIMO2,
         LQ_MAX,
 };
 
-#define is_legacy(tbl) (((tbl) == LQ_G) || ((tbl) == LQ_A))
-#define is_siso(tbl) ((tbl) == LQ_SISO)
-#define is_mimo2(tbl) ((tbl) == LQ_MIMO2)
-#define is_mimo(tbl) is_mimo2(tbl)
-#define is_Ht(tbl) (is_siso(tbl) || is_mimo(tbl))
-#define is_a_band(tbl) ((tbl) == LQ_A)
-#define is_g_and(tbl) ((tbl) == LQ_G)
+#define is_legacy(tbl) (((tbl) == LQ_LEGACY_G) || ((tbl) == LQ_LEGACY_A))
+#define is_ht_siso(tbl) ((tbl) == LQ_HT_SISO)
+#define is_ht_mimo2(tbl) ((tbl) == LQ_HT_MIMO2)
+#define is_vht_siso(tbl) ((tbl) == LQ_VHT_SISO)
+#define is_vht_mimo2(tbl) ((tbl) == LQ_VHT_MIMO2)
+#define is_siso(tbl) (is_ht_siso(tbl) || is_vht_siso(tbl))
+#define is_mimo2(tbl) (is_ht_mimo2(tbl) || is_vht_mimo2(tbl))
+#define is_mimo(tbl) (is_mimo2(tbl))
+#define is_ht(tbl) (is_ht_siso(tbl) || is_ht_mimo2(tbl))
+#define is_vht(tbl) (is_vht_siso(tbl) || is_vht_mimo2(tbl))
+#define is_a_band(tbl) ((tbl) == LQ_LEGACY_A)
+#define is_g_band(tbl) ((tbl) == LQ_LEGACY_G)
+
+#define is_ht20(tbl) (tbl->bw == RATE_MCS_CHAN_WIDTH_20)
+#define is_ht40(tbl) (tbl->bw == RATE_MCS_CHAN_WIDTH_40)
+#define is_ht80(tbl) (tbl->bw == RATE_MCS_CHAN_WIDTH_80)
 
 #define IWL_MAX_MCS_DISPLAY_SIZE        12
 
@@ -232,7 +270,7 @@ struct iwl_scale_tbl_info {
         enum iwl_table_type lq_type;
         u8 ant_type;
         u8 is_SGI;      /* 1 = short guard interval */
-        u8 is_ht40;     /* 1 = 40 MHz channel width */
+        u32 bw;         /* channel bandwidth; RATE_MCS_CHAN_WIDTH_XX */
         u8 action;      /* change modulation; IWL_[LEGACY/SISO/MIMO]_SWITCH_* */
         u8 max_search;  /* maximun number of tables we can search */
         s32 *expected_tpt;      /* throughput metrics; expected_tpt_G, etc. */
@@ -262,7 +300,7 @@ struct iwl_lq_sta {
         u64 flush_timer;        /* time staying in mode before new search */
 
         u8 action_counter;      /* # mode-switch actions tried */
-        u8 is_green;
+        bool is_vht;
         enum ieee80211_band band;
 
         /* The following are bitmaps of rates; IWL_RATE_6M_MASK, etc. */
diff --git a/drivers/net/wireless/iwlwifi/mvm/rx.c b/drivers/net/wireless/iwlwifi/mvm/rx.c
index 2a8cb5a60535..a4af5019a496 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rx.c
@@ -422,6 +422,27 @@ static void iwl_mvm_stat_iterator(void *_data, u8 *mac,
 
         mvmvif->bf_data.ave_beacon_signal = sig;
 
+        /* BT Coex */
+        if (mvmvif->bf_data.bt_coex_min_thold !=
+            mvmvif->bf_data.bt_coex_max_thold) {
+                last_event = mvmvif->bf_data.last_bt_coex_event;
+                if (sig > mvmvif->bf_data.bt_coex_max_thold &&
+                    (last_event <= mvmvif->bf_data.bt_coex_min_thold ||
+                     last_event == 0)) {
+                        mvmvif->bf_data.last_bt_coex_event = sig;
+                        IWL_DEBUG_RX(mvm, "cqm_iterator bt coex high %d\n",
+                                     sig);
+                        iwl_mvm_bt_rssi_event(mvm, vif, RSSI_EVENT_HIGH);
+                } else if (sig < mvmvif->bf_data.bt_coex_min_thold &&
+                           (last_event >= mvmvif->bf_data.bt_coex_max_thold ||
+                            last_event == 0)) {
+                        mvmvif->bf_data.last_bt_coex_event = sig;
+                        IWL_DEBUG_RX(mvm, "cqm_iterator bt coex low %d\n",
+                                     sig);
+                        iwl_mvm_bt_rssi_event(mvm, vif, RSSI_EVENT_LOW);
+                }
+        }
+
         if (!(vif->driver_flags & IEEE80211_VIF_SUPPORTS_CQM_RSSI))
                 return;
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/scan.c b/drivers/net/wireless/iwlwifi/mvm/scan.c
index 621fb71f282a..dff7592e1ff8 100644
--- a/drivers/net/wireless/iwlwifi/mvm/scan.c
+++ b/drivers/net/wireless/iwlwifi/mvm/scan.c
@@ -74,8 +74,12 @@
 static inline __le16 iwl_mvm_scan_rx_chain(struct iwl_mvm *mvm)
 {
         u16 rx_chain;
-        u8 rx_ant = iwl_fw_valid_rx_ant(mvm->fw);
+        u8 rx_ant;
 
+        if (mvm->scan_rx_ant != ANT_NONE)
+                rx_ant = mvm->scan_rx_ant;
+        else
+                rx_ant = iwl_fw_valid_rx_ant(mvm->fw);
         rx_chain = rx_ant << PHY_RX_CHAIN_VALID_POS;
         rx_chain |= rx_ant << PHY_RX_CHAIN_FORCE_MIMO_SEL_POS;
         rx_chain |= rx_ant << PHY_RX_CHAIN_FORCE_SEL_POS;
@@ -93,10 +97,10 @@ static inline __le32 iwl_mvm_scan_max_out_time(struct ieee80211_vif *vif)
 
 static inline __le32 iwl_mvm_scan_suspend_time(struct ieee80211_vif *vif)
 {
-        if (vif->bss_conf.assoc)
-                return cpu_to_le32(vif->bss_conf.beacon_int);
-        else
+        if (!vif->bss_conf.assoc)
                 return 0;
+
+        return cpu_to_le32(ieee80211_tu_to_usec(vif->bss_conf.beacon_int));
 }
 
 static inline __le32
@@ -133,11 +137,12 @@ iwl_mvm_scan_rate_n_flags(struct iwl_mvm *mvm, enum ieee80211_band band,
  * request.
  */
 static void iwl_mvm_scan_fill_ssids(struct iwl_scan_cmd *cmd,
-                                    struct cfg80211_scan_request *req)
+                                    struct cfg80211_scan_request *req,
+                                    int first)
 {
         int fw_idx, req_idx;
 
-        for (req_idx = req->n_ssids - 1, fw_idx = 0; req_idx > 0;
+        for (req_idx = req->n_ssids - 1, fw_idx = 0; req_idx >= first;
              req_idx--, fw_idx++) {
                 cmd->direct_scan[fw_idx].id = WLAN_EID_SSID;
                 cmd->direct_scan[fw_idx].len = req->ssids[req_idx].ssid_len;
@@ -153,9 +158,9 @@ static void iwl_mvm_scan_fill_ssids(struct iwl_scan_cmd *cmd,
  * just to notify that this scan is active and not passive.
  * In order to notify the FW of the number of SSIDs we wish to scan (including
  * the zero-length one), we need to set the corresponding bits in chan->type,
- * one for each SSID, and set the active bit (first). The first SSID is already
- * included in the probe template, so we need to set only req->n_ssids - 1 bits
- * in addition to the first bit.
+ * one for each SSID, and set the active bit (first). If the first SSID is
+ * already included in the probe template, so we need to set only
+ * req->n_ssids - 1 bits in addition to the first bit.
  */
 static u16 iwl_mvm_get_active_dwell(enum ieee80211_band band, int n_ssids)
 {
@@ -170,7 +175,8 @@ static u16 iwl_mvm_get_passive_dwell(enum ieee80211_band band)
 }
 
 static void iwl_mvm_scan_fill_channels(struct iwl_scan_cmd *cmd,
-                                       struct cfg80211_scan_request *req)
+                                       struct cfg80211_scan_request *req,
+                                       bool basic_ssid)
 {
         u16 passive_dwell = iwl_mvm_get_passive_dwell(req->channels[0]->band);
         u16 active_dwell = iwl_mvm_get_active_dwell(req->channels[0]->band,
@@ -178,10 +184,14 @@ static void iwl_mvm_scan_fill_channels(struct iwl_scan_cmd *cmd,
         struct iwl_scan_channel *chan = (struct iwl_scan_channel *)
                 (cmd->data + le16_to_cpu(cmd->tx_cmd.len));
         int i;
+        int type = BIT(req->n_ssids) - 1;
+
+        if (!basic_ssid)
+                type |= BIT(req->n_ssids);
 
         for (i = 0; i < cmd->channel_count; i++) {
                 chan->channel = cpu_to_le16(req->channels[i]->hw_value);
-                chan->type = cpu_to_le32(BIT(req->n_ssids) - 1);
+                chan->type = cpu_to_le32(type);
                 if (req->channels[i]->flags & IEEE80211_CHAN_PASSIVE_SCAN)
                         chan->type &= cpu_to_le32(~SCAN_CHANNEL_TYPE_ACTIVE);
                 chan->active_dwell = cpu_to_le16(active_dwell);
@@ -268,6 +278,8 @@ int iwl_mvm_scan_request(struct iwl_mvm *mvm,
         u32 status;
         int ssid_len = 0;
         u8 *ssid = NULL;
+        bool basic_ssid = !(mvm->fw->ucode_capa.flags &
+                           IWL_UCODE_TLV_FLAGS_NO_BASIC_SSID);
 
         lockdep_assert_held(&mvm->mutex);
         BUG_ON(mvm->scan_cmd == NULL);
@@ -302,14 +314,16 @@ int iwl_mvm_scan_request(struct iwl_mvm *mvm,
         if (req->n_ssids > 0) {
                 cmd->passive2active = cpu_to_le16(1);
                 cmd->scan_flags |= SCAN_FLAGS_PASSIVE2ACTIVE;
-                ssid = req->ssids[0].ssid;
-                ssid_len = req->ssids[0].ssid_len;
+                if (basic_ssid) {
+                        ssid = req->ssids[0].ssid;
+                        ssid_len = req->ssids[0].ssid_len;
+                }
         } else {
                 cmd->passive2active = 0;
                 cmd->scan_flags &= ~SCAN_FLAGS_PASSIVE2ACTIVE;
         }
 
-        iwl_mvm_scan_fill_ssids(cmd, req);
+        iwl_mvm_scan_fill_ssids(cmd, req, basic_ssid ? 1 : 0);
 
         cmd->tx_cmd.tx_flags = cpu_to_le32(TX_CMD_FLG_SEQ_CTL);
         cmd->tx_cmd.sta_id = mvm->aux_sta.sta_id;
@@ -326,7 +340,7 @@ int iwl_mvm_scan_request(struct iwl_mvm *mvm,
                             req->ie, req->ie_len,
                             mvm->fw->ucode_capa.max_probe_length));
 
-        iwl_mvm_scan_fill_channels(cmd, req);
+        iwl_mvm_scan_fill_channels(cmd, req, basic_ssid);
 
         cmd->len = cpu_to_le16(sizeof(struct iwl_scan_cmd) +
                 le16_to_cpu(cmd->tx_cmd.len) +
@@ -377,6 +391,21 @@ int iwl_mvm_rx_scan_complete(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
         return 0;
 }
 
+int iwl_mvm_rx_sched_scan_results(struct iwl_mvm *mvm,
+                                  struct iwl_rx_cmd_buffer *rxb,
+                                  struct iwl_device_cmd *cmd)
+{
+        struct iwl_rx_packet *pkt = rxb_addr(rxb);
+        struct iwl_sched_scan_results *notif = (void *)pkt->data;
+
+        if (notif->client_bitmap & SCAN_CLIENT_SCHED_SCAN) {
+                IWL_DEBUG_SCAN(mvm, "Scheduled scan results\n");
+                ieee80211_sched_scan_results(mvm->hw);
+        }
+
+        return 0;
+}
+
 static bool iwl_mvm_scan_abort_notif(struct iwl_notif_wait_data *notif_wait,
                                      struct iwl_rx_packet *pkt, void *data)
 {
@@ -447,3 +476,406 @@ void iwl_mvm_cancel_scan(struct iwl_mvm *mvm)
 out_remove_notif:
         iwl_remove_notification(&mvm->notif_wait, &wait_scan_abort);
 }
+
+int iwl_mvm_rx_scan_offload_complete_notif(struct iwl_mvm *mvm,
+                                           struct iwl_rx_cmd_buffer *rxb,
+                                           struct iwl_device_cmd *cmd)
+{
+        struct iwl_rx_packet *pkt = rxb_addr(rxb);
+        struct iwl_scan_offload_complete *scan_notif = (void *)pkt->data;
+
+        IWL_DEBUG_SCAN(mvm, "Scheduled scan completed, status %s\n",
+                       scan_notif->status == IWL_SCAN_OFFLOAD_COMPLETED ?
+                       "completed" : "aborted");
+
+        mvm->scan_status = IWL_MVM_SCAN_NONE;
+        ieee80211_sched_scan_stopped(mvm->hw);
+
+        return 0;
+}
+
+static void iwl_scan_offload_build_tx_cmd(struct iwl_mvm *mvm,
+                                          struct ieee80211_vif *vif,
+                                          struct ieee80211_sched_scan_ies *ies,
+                                          enum ieee80211_band band,
+                                          struct iwl_tx_cmd *cmd,
+                                          u8 *data)
+{
+        u16 cmd_len;
+
+        cmd->tx_flags = cpu_to_le32(TX_CMD_FLG_SEQ_CTL);
+        cmd->life_time = cpu_to_le32(TX_CMD_LIFE_TIME_INFINITE);
+        cmd->sta_id = mvm->aux_sta.sta_id;
+
+        cmd->rate_n_flags = iwl_mvm_scan_rate_n_flags(mvm, band, false);
+
+        cmd_len = iwl_mvm_fill_probe_req((struct ieee80211_mgmt *)data,
+                                         vif->addr,
+                                         1, NULL, 0,
+                                         ies->ie[band], ies->len[band],
+                                         SCAN_OFFLOAD_PROBE_REQ_SIZE);
+        cmd->len = cpu_to_le16(cmd_len);
+}
+
+static void iwl_build_scan_cmd(struct iwl_mvm *mvm,
+                               struct ieee80211_vif *vif,
+                               struct cfg80211_sched_scan_request *req,
+                               struct iwl_scan_offload_cmd *scan)
+{
+        scan->channel_count =
+                mvm->nvm_data->bands[IEEE80211_BAND_2GHZ].n_channels +
+                mvm->nvm_data->bands[IEEE80211_BAND_5GHZ].n_channels;
+        scan->quiet_time = cpu_to_le16(IWL_ACTIVE_QUIET_TIME);
+        scan->quiet_plcp_th = cpu_to_le16(IWL_PLCP_QUIET_THRESH);
+        scan->good_CRC_th = IWL_GOOD_CRC_TH_DEFAULT;
+        scan->rx_chain = iwl_mvm_scan_rx_chain(mvm);
+        scan->max_out_time = cpu_to_le32(200 * 1024);
+        scan->suspend_time = iwl_mvm_scan_suspend_time(vif);
+        scan->filter_flags |= cpu_to_le32(MAC_FILTER_ACCEPT_GRP |
+                                          MAC_FILTER_IN_BEACON);
+        scan->scan_type = cpu_to_le32(SCAN_TYPE_BACKGROUND);
+        scan->rep_count = cpu_to_le32(1);
+}
+
+static int iwl_ssid_exist(u8 *ssid, u8 ssid_len, struct iwl_ssid_ie *ssid_list)
+{
+        int i;
+
+        for (i = 0; i < PROBE_OPTION_MAX; i++) {
+                if (!ssid_list[i].len)
+                        break;
+                if (ssid_list[i].len == ssid_len &&
+                    !memcmp(ssid_list->ssid, ssid, ssid_len))
+                        return i;
+        }
+        return -1;
+}
+
+static void iwl_scan_offload_build_ssid(struct cfg80211_sched_scan_request *req,
+                                        struct iwl_scan_offload_cmd *scan,
+                                        u32 *ssid_bitmap)
+{
+        int i, j;
+        int index;
+
+        /*
+         * copy SSIDs from match list.
+         * iwl_config_sched_scan_profiles() uses the order of these ssids to
+         * config match list.
+         */
+        for (i = 0; i < req->n_match_sets && i < PROBE_OPTION_MAX; i++) {
+                scan->direct_scan[i].id = WLAN_EID_SSID;
+                scan->direct_scan[i].len = req->match_sets[i].ssid.ssid_len;
+                memcpy(scan->direct_scan[i].ssid, req->match_sets[i].ssid.ssid,
+                       scan->direct_scan[i].len);
+        }
+
+        /* add SSIDs from scan SSID list */
+        *ssid_bitmap = 0;
+        for (j = 0; j < req->n_ssids && i < PROBE_OPTION_MAX; j++) {
+                index = iwl_ssid_exist(req->ssids[j].ssid,
+                                       req->ssids[j].ssid_len,
+                                       scan->direct_scan);
+                if (index < 0) {
+                        if (!req->ssids[j].ssid_len)
+                                continue;
+                        scan->direct_scan[i].id = WLAN_EID_SSID;
+                        scan->direct_scan[i].len = req->ssids[j].ssid_len;
+                        memcpy(scan->direct_scan[i].ssid, req->ssids[j].ssid,
+                               scan->direct_scan[i].len);
+                        *ssid_bitmap |= BIT(i + 1);
+                        i++;
+                } else {
+                        *ssid_bitmap |= BIT(index + 1);
+                }
+        }
+}
+
+static void iwl_build_channel_cfg(struct iwl_mvm *mvm,
+                                  struct cfg80211_sched_scan_request *req,
+                                  struct iwl_scan_channel_cfg *channels,
+                                  enum ieee80211_band band,
+                                  int *head, int *tail,
+                                  u32 ssid_bitmap)
+{
+        struct ieee80211_supported_band *s_band;
+        int n_probes = req->n_ssids;
+        int n_channels = req->n_channels;
+        u8 active_dwell, passive_dwell;
+        int i, j, index = 0;
+        bool partial;
+
+        /*
+         * We have to configure all supported channels, even if we don't want to
+         * scan on them, but we have to send channels in the order that we want
+         * to scan. So add requested channels to head of the list and others to
+         * the end.
+        */
+        active_dwell = iwl_mvm_get_active_dwell(band, n_probes);
+        passive_dwell = iwl_mvm_get_passive_dwell(band);
+        s_band = &mvm->nvm_data->bands[band];
+
+        for (i = 0; i < s_band->n_channels && *head <= *tail; i++) {
+                partial = false;
+                for (j = 0; j < n_channels; j++)
+                        if (s_band->channels[i].center_freq ==
+                                                req->channels[j]->center_freq) {
+                                index = *head;
+                                (*head)++;
+                                /*
+                                 * Channels that came with the request will be
+                                 * in partial scan .
+                                 */
+                                partial = true;
+                                break;
+                        }
+                if (!partial) {
+                        index = *tail;
+                        (*tail)--;
+                }
+                channels->channel_number[index] =
+                        cpu_to_le16(ieee80211_frequency_to_channel(
+                                        s_band->channels[i].center_freq));
+                channels->dwell_time[index][0] = active_dwell;
+                channels->dwell_time[index][1] = passive_dwell;
+
+                channels->iter_count[index] = cpu_to_le16(1);
+                channels->iter_interval[index] = 0;
+
+                if (!(s_band->channels[i].flags & IEEE80211_CHAN_PASSIVE_SCAN))
+                        channels->type[index] |=
+                                cpu_to_le32(IWL_SCAN_OFFLOAD_CHANNEL_ACTIVE);
+
+                channels->type[index] |=
+                                cpu_to_le32(IWL_SCAN_OFFLOAD_CHANNEL_FULL);
+                if (partial)
+                        channels->type[index] |=
+                                cpu_to_le32(IWL_SCAN_OFFLOAD_CHANNEL_PARTIAL);
+
+                if (s_band->channels[i].flags & IEEE80211_CHAN_NO_HT40)
+                        channels->type[index] |=
+                                cpu_to_le32(IWL_SCAN_OFFLOAD_CHANNEL_NARROW);
+
+                /* scan for all SSIDs from req->ssids */
+                channels->type[index] |= cpu_to_le32(ssid_bitmap);
+        }
+}
+
+int iwl_mvm_config_sched_scan(struct iwl_mvm *mvm,
+                              struct ieee80211_vif *vif,
+                              struct cfg80211_sched_scan_request *req,
+                              struct ieee80211_sched_scan_ies *ies)
+{
+        int supported_bands = 0;
+        int band_2ghz = mvm->nvm_data->bands[IEEE80211_BAND_2GHZ].n_channels;
+        int band_5ghz = mvm->nvm_data->bands[IEEE80211_BAND_5GHZ].n_channels;
+        int head = 0;
+        int tail = band_2ghz + band_5ghz;
+        u32 ssid_bitmap;
+        int cmd_len;
+        int ret;
+
+        struct iwl_scan_offload_cfg *scan_cfg;
+        struct iwl_host_cmd cmd = {
+                .id = SCAN_OFFLOAD_CONFIG_CMD,
+                .flags = CMD_SYNC,
+        };
+
+        lockdep_assert_held(&mvm->mutex);
+
+        if (band_2ghz)
+                supported_bands++;
+        if (band_5ghz)
+                supported_bands++;
+
+        cmd_len = sizeof(struct iwl_scan_offload_cfg) +
+                                supported_bands * SCAN_OFFLOAD_PROBE_REQ_SIZE;
+
+        scan_cfg = kzalloc(cmd_len, GFP_KERNEL);
+        if (!scan_cfg)
+                return -ENOMEM;
+
+        iwl_build_scan_cmd(mvm, vif, req, &scan_cfg->scan_cmd);
+        scan_cfg->scan_cmd.len = cpu_to_le16(cmd_len);
+
+        iwl_scan_offload_build_ssid(req, &scan_cfg->scan_cmd, &ssid_bitmap);
+        /* build tx frames for supported bands */
+        if (band_2ghz) {
+                iwl_scan_offload_build_tx_cmd(mvm, vif, ies,
+                                              IEEE80211_BAND_2GHZ,
+                                              &scan_cfg->scan_cmd.tx_cmd[0],
+                                              scan_cfg->data);
+                iwl_build_channel_cfg(mvm, req, &scan_cfg->channel_cfg,
+                                      IEEE80211_BAND_2GHZ, &head, &tail,
+                                      ssid_bitmap);
+        }
+        if (band_5ghz) {
+                iwl_scan_offload_build_tx_cmd(mvm, vif, ies,
+                                              IEEE80211_BAND_5GHZ,
+                                              &scan_cfg->scan_cmd.tx_cmd[1],
+                                              scan_cfg->data +
+                                                SCAN_OFFLOAD_PROBE_REQ_SIZE);
+                iwl_build_channel_cfg(mvm, req, &scan_cfg->channel_cfg,
+                                      IEEE80211_BAND_5GHZ, &head, &tail,
+                                      ssid_bitmap);
+        }
+
+        cmd.data[0] = scan_cfg;
+        cmd.len[0] = cmd_len;
+        cmd.dataflags[0] = IWL_HCMD_DFL_NOCOPY;
+
+        IWL_DEBUG_SCAN(mvm, "Sending scheduled scan config\n");
+
+        ret = iwl_mvm_send_cmd(mvm, &cmd);
+        kfree(scan_cfg);
+        return ret;
+}
+
+int iwl_mvm_config_sched_scan_profiles(struct iwl_mvm *mvm,
+                                       struct cfg80211_sched_scan_request *req)
+{
+        struct iwl_scan_offload_profile *profile;
+        struct iwl_scan_offload_profile_cfg *profile_cfg;
+        struct iwl_scan_offload_blacklist *blacklist;
+        struct iwl_host_cmd cmd = {
+                .id = SCAN_OFFLOAD_UPDATE_PROFILES_CMD,
+                .flags = CMD_SYNC,
+                .len[1] = sizeof(*profile_cfg),
+                .dataflags[0] = IWL_HCMD_DFL_NOCOPY,
+                .dataflags[1] = IWL_HCMD_DFL_NOCOPY,
+        };
+        int blacklist_len;
+        int i;
+        int ret;
+
+        if (WARN_ON(req->n_match_sets > IWL_SCAN_MAX_PROFILES))
+                        return -EIO;
+
+        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_SHORT_BL)
+                blacklist_len = IWL_SCAN_SHORT_BLACKLIST_LEN;
+        else
+                blacklist_len = IWL_SCAN_MAX_BLACKLIST_LEN;
+
+        blacklist = kzalloc(sizeof(*blacklist) * blacklist_len, GFP_KERNEL);
+        if (!blacklist)
+                return -ENOMEM;
+
+        profile_cfg = kzalloc(sizeof(*profile_cfg), GFP_KERNEL);
+        if (!profile_cfg) {
+                ret = -ENOMEM;
+                goto free_blacklist;
+        }
+
+        cmd.data[0] = blacklist;
+        cmd.len[0] = sizeof(*blacklist) * blacklist_len;
+        cmd.data[1] = profile_cfg;
+
+        /* No blacklist configuration */
+
+        profile_cfg->num_profiles = req->n_match_sets;
+        profile_cfg->active_clients = SCAN_CLIENT_SCHED_SCAN;
+        profile_cfg->pass_match = SCAN_CLIENT_SCHED_SCAN;
+        profile_cfg->match_notify = SCAN_CLIENT_SCHED_SCAN;
+
+        for (i = 0; i < req->n_match_sets; i++) {
+                profile = &profile_cfg->profiles[i];
+                profile->ssid_index = i;
+                /* Support any cipher and auth algorithm */
+                profile->unicast_cipher = 0xff;
+                profile->auth_alg = 0xff;
+                profile->network_type = IWL_NETWORK_TYPE_ANY;
+                profile->band_selection = IWL_SCAN_OFFLOAD_SELECT_ANY;
+                profile->client_bitmap = SCAN_CLIENT_SCHED_SCAN;
+        }
+
+        IWL_DEBUG_SCAN(mvm, "Sending scheduled scan profile config\n");
+
+        ret = iwl_mvm_send_cmd(mvm, &cmd);
+        kfree(profile_cfg);
+free_blacklist:
+        kfree(blacklist);
+
+        return ret;
+}
+
+int iwl_mvm_sched_scan_start(struct iwl_mvm *mvm,
+                             struct cfg80211_sched_scan_request *req)
+{
+        struct iwl_scan_offload_req scan_req = {
+                .watchdog = IWL_SCHED_SCAN_WATCHDOG,
+
+                .schedule_line[0].iterations = IWL_FAST_SCHED_SCAN_ITERATIONS,
+                .schedule_line[0].delay = req->interval / 1000,
+                .schedule_line[0].full_scan_mul = 1,
+
+                .schedule_line[1].iterations = 0xff,
+                .schedule_line[1].delay = req->interval / 1000,
+                .schedule_line[1].full_scan_mul = IWL_FULL_SCAN_MULTIPLIER,
+        };
+
+        if (req->n_match_sets && req->match_sets[0].ssid.ssid_len) {
+                IWL_DEBUG_SCAN(mvm,
+                               "Sending scheduled scan with filtering, filter len %d\n",
+                               req->n_match_sets);
+                scan_req.flags |=
+                                cpu_to_le16(IWL_SCAN_OFFLOAD_FLAG_FILTER_SSID);
+        } else {
+                IWL_DEBUG_SCAN(mvm,
+                               "Sending Scheduled scan without filtering\n");
+        }
+
+        return iwl_mvm_send_cmd_pdu(mvm, SCAN_OFFLOAD_REQUEST_CMD, CMD_SYNC,
+                                    sizeof(scan_req), &scan_req);
+}
+
+static int iwl_mvm_send_sched_scan_abort(struct iwl_mvm *mvm)
+{
+        int ret;
+        struct iwl_host_cmd cmd = {
+                .id = SCAN_OFFLOAD_ABORT_CMD,
+                .flags = CMD_SYNC,
+        };
+        u32 status;
+
+        /* Exit instantly with error when device is not ready
+         * to receive scan abort command or it does not perform
+         * scheduled scan currently */
+        if (mvm->scan_status != IWL_MVM_SCAN_SCHED)
+                return -EIO;
+
+        ret = iwl_mvm_send_cmd_status(mvm, &cmd, &status);
+        if (ret)
+                return ret;
+
+        if (status != CAN_ABORT_STATUS) {
+                /*
+                 * The scan abort will return 1 for success or
+                 * 2 for "failure".  A failure condition can be
+                 * due to simply not being in an active scan which
+                 * can occur if we send the scan abort before the
+                 * microcode has notified us that a scan is completed.
+                 */
+                IWL_DEBUG_SCAN(mvm, "SCAN OFFLOAD ABORT ret %d.\n", status);
+                ret = -EIO;
+        }
+
+        return ret;
+}
+
+void iwl_mvm_sched_scan_stop(struct iwl_mvm *mvm)
+{
+        int ret;
+
+        lockdep_assert_held(&mvm->mutex);
+
+        if (mvm->scan_status != IWL_MVM_SCAN_SCHED) {
+                IWL_DEBUG_SCAN(mvm, "No offloaded scan to stop\n");
+                return;
+        }
+
+        ret = iwl_mvm_send_sched_scan_abort(mvm);
+        if (ret)
+                IWL_DEBUG_SCAN(mvm, "Send stop offload scan failed %d\n", ret);
+        else
+                IWL_DEBUG_SCAN(mvm, "Successfully sent stop offload scan\n");
+}
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.c b/drivers/net/wireless/iwlwifi/mvm/sta.c
index 44add291531b..329952363a54 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.c
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.c
@@ -66,6 +66,115 @@
 #include "sta.h"
 #include "rs.h"
 
+static void iwl_mvm_add_sta_cmd_v6_to_v5(struct iwl_mvm_add_sta_cmd_v6 *cmd_v6,
+                                         struct iwl_mvm_add_sta_cmd_v5 *cmd_v5)
+{
+        memset(cmd_v5, 0, sizeof(*cmd_v5));
+
+        cmd_v5->add_modify = cmd_v6->add_modify;
+        cmd_v5->tid_disable_tx = cmd_v6->tid_disable_tx;
+        cmd_v5->mac_id_n_color = cmd_v6->mac_id_n_color;
+        memcpy(cmd_v5->addr, cmd_v6->addr, ETH_ALEN);
+        cmd_v5->sta_id = cmd_v6->sta_id;
+        cmd_v5->modify_mask = cmd_v6->modify_mask;
+        cmd_v5->station_flags = cmd_v6->station_flags;
+        cmd_v5->station_flags_msk = cmd_v6->station_flags_msk;
+        cmd_v5->add_immediate_ba_tid = cmd_v6->add_immediate_ba_tid;
+        cmd_v5->remove_immediate_ba_tid = cmd_v6->remove_immediate_ba_tid;
+        cmd_v5->add_immediate_ba_ssn = cmd_v6->add_immediate_ba_ssn;
+        cmd_v5->sleep_tx_count = cmd_v6->sleep_tx_count;
+        cmd_v5->sleep_state_flags = cmd_v6->sleep_state_flags;
+        cmd_v5->assoc_id = cmd_v6->assoc_id;
+        cmd_v5->beamform_flags = cmd_v6->beamform_flags;
+        cmd_v5->tfd_queue_msk = cmd_v6->tfd_queue_msk;
+}
+
+static void
+iwl_mvm_add_sta_key_to_add_sta_cmd_v5(struct iwl_mvm_add_sta_key_cmd *key_cmd,
+                                      struct iwl_mvm_add_sta_cmd_v5 *sta_cmd,
+                                      u32 mac_id_n_color)
+{
+        memset(sta_cmd, 0, sizeof(*sta_cmd));
+
+        sta_cmd->sta_id = key_cmd->sta_id;
+        sta_cmd->add_modify = STA_MODE_MODIFY;
+        sta_cmd->modify_mask = STA_MODIFY_KEY;
+        sta_cmd->mac_id_n_color = cpu_to_le32(mac_id_n_color);
+
+        sta_cmd->key.key_offset = key_cmd->key_offset;
+        sta_cmd->key.key_flags = key_cmd->key_flags;
+        memcpy(sta_cmd->key.key, key_cmd->key, sizeof(sta_cmd->key.key));
+        sta_cmd->key.tkip_rx_tsc_byte2 = key_cmd->tkip_rx_tsc_byte2;
+        memcpy(sta_cmd->key.tkip_rx_ttak, key_cmd->tkip_rx_ttak,
+               sizeof(sta_cmd->key.tkip_rx_ttak));
+}
+
+static int iwl_mvm_send_add_sta_cmd_status(struct iwl_mvm *mvm,
+                                           struct iwl_mvm_add_sta_cmd_v6 *cmd,
+                                           int *status)
+{
+        struct iwl_mvm_add_sta_cmd_v5 cmd_v5;
+
+        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_STA_KEY_CMD)
+                return iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(*cmd),
+                                                   cmd, status);
+
+        iwl_mvm_add_sta_cmd_v6_to_v5(cmd, &cmd_v5);
+
+        return iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd_v5),
+                                           &cmd_v5, status);
+}
+
+static int iwl_mvm_send_add_sta_cmd(struct iwl_mvm *mvm, u32 flags,
+                                    struct iwl_mvm_add_sta_cmd_v6 *cmd)
+{
+        struct iwl_mvm_add_sta_cmd_v5 cmd_v5;
+
+        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_STA_KEY_CMD)
+                return iwl_mvm_send_cmd_pdu(mvm, ADD_STA, flags,
+                                            sizeof(*cmd), cmd);
+
+        iwl_mvm_add_sta_cmd_v6_to_v5(cmd, &cmd_v5);
+
+        return iwl_mvm_send_cmd_pdu(mvm, ADD_STA, flags, sizeof(cmd_v5),
+                                    &cmd_v5);
+}
+
+static int
+iwl_mvm_send_add_sta_key_cmd_status(struct iwl_mvm *mvm,
+                                    struct iwl_mvm_add_sta_key_cmd *cmd,
+                                    u32 mac_id_n_color,
+                                    int *status)
+{
+        struct iwl_mvm_add_sta_cmd_v5 sta_cmd;
+
+        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_STA_KEY_CMD)
+                return iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA_KEY,
+                                                   sizeof(*cmd), cmd, status);
+
+        iwl_mvm_add_sta_key_to_add_sta_cmd_v5(cmd, &sta_cmd, mac_id_n_color);
+
+        return iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(sta_cmd),
+                                           &sta_cmd, status);
+}
+
+static int iwl_mvm_send_add_sta_key_cmd(struct iwl_mvm *mvm,
+                                        u32 flags,
+                                        struct iwl_mvm_add_sta_key_cmd *cmd,
+                                        u32 mac_id_n_color)
+{
+        struct iwl_mvm_add_sta_cmd_v5 sta_cmd;
+
+        if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_STA_KEY_CMD)
+                return iwl_mvm_send_cmd_pdu(mvm, ADD_STA_KEY, flags,
+                                            sizeof(*cmd), cmd);
+
+        iwl_mvm_add_sta_key_to_add_sta_cmd_v5(cmd, &sta_cmd, mac_id_n_color);
+
+        return iwl_mvm_send_cmd_pdu(mvm, ADD_STA, flags, sizeof(sta_cmd),
+                                    &sta_cmd);
+}
+
 static int iwl_mvm_find_free_sta_id(struct iwl_mvm *mvm)
 {
         int sta_id;
@@ -87,7 +196,7 @@ int iwl_mvm_sta_send_to_fw(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
                            bool update)
 {
         struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
-        struct iwl_mvm_add_sta_cmd add_sta_cmd;
+        struct iwl_mvm_add_sta_cmd_v6 add_sta_cmd;
         int ret;
         u32 status;
         u32 agg_size = 0, mpdu_dens = 0;
@@ -175,8 +284,7 @@ int iwl_mvm_sta_send_to_fw(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
                 cpu_to_le32(mpdu_dens << STA_FLG_AGG_MPDU_DENS_SHIFT);
 
         status = ADD_STA_SUCCESS;
-        ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(add_sta_cmd),
-                                          &add_sta_cmd, &status);
+        ret = iwl_mvm_send_add_sta_cmd_status(mvm, &add_sta_cmd, &status);
         if (ret)
                 return ret;
 
@@ -229,8 +337,12 @@ int iwl_mvm_add_sta(struct iwl_mvm *mvm,
                 if (vif->hw_queue[i] != IEEE80211_INVAL_HW_QUEUE)
                         mvm_sta->tfd_queue_msk |= BIT(vif->hw_queue[i]);
 
-        /* for HW restart - need to reset the seq_number etc... */
-        memset(mvm_sta->tid_data, 0, sizeof(mvm_sta->tid_data));
+        /* for HW restart - reset everything but the sequence number */
+        for (i = 0; i < IWL_MAX_TID_COUNT; i++) {
+                u16 seq = mvm_sta->tid_data[i].seq_number;
+                memset(&mvm_sta->tid_data[i], 0, sizeof(mvm_sta->tid_data[i]));
+                mvm_sta->tid_data[i].seq_number = seq;
+        }
 
         ret = iwl_mvm_sta_send_to_fw(mvm, sta, false);
         if (ret)
@@ -256,7 +368,7 @@ int iwl_mvm_update_sta(struct iwl_mvm *mvm,
 int iwl_mvm_drain_sta(struct iwl_mvm *mvm, struct iwl_mvm_sta *mvmsta,
                       bool drain)
 {
-        struct iwl_mvm_add_sta_cmd cmd = {};
+        struct iwl_mvm_add_sta_cmd_v6 cmd = {};
         int ret;
         u32 status;
 
@@ -269,8 +381,7 @@ int iwl_mvm_drain_sta(struct iwl_mvm *mvm, struct iwl_mvm_sta *mvmsta,
         cmd.station_flags_msk = cpu_to_le32(STA_FLG_DRAIN_FLOW);
 
         status = ADD_STA_SUCCESS;
-        ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd),
-                                          &cmd, &status);
+        ret = iwl_mvm_send_add_sta_cmd_status(mvm, &cmd, &status);
         if (ret)
                 return ret;
 
@@ -469,13 +580,13 @@ static int iwl_mvm_add_int_sta_common(struct iwl_mvm *mvm,
                                       const u8 *addr,
                                       u16 mac_id, u16 color)
 {
-        struct iwl_mvm_add_sta_cmd cmd;
+        struct iwl_mvm_add_sta_cmd_v6 cmd;
         int ret;
         u32 status;
 
         lockdep_assert_held(&mvm->mutex);
 
-        memset(&cmd, 0, sizeof(struct iwl_mvm_add_sta_cmd));
+        memset(&cmd, 0, sizeof(struct iwl_mvm_add_sta_cmd_v6));
         cmd.sta_id = sta->sta_id;
         cmd.mac_id_n_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mac_id,
                                                              color));
@@ -485,8 +596,7 @@ static int iwl_mvm_add_int_sta_common(struct iwl_mvm *mvm,
         if (addr)
                 memcpy(cmd.addr, addr, ETH_ALEN);
 
-        ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd),
-                                          &cmd, &status);
+        ret = iwl_mvm_send_add_sta_cmd_status(mvm, &cmd, &status);
         if (ret)
                 return ret;
 
@@ -534,10 +644,14 @@ int iwl_mvm_send_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
                            struct iwl_mvm_int_sta *bsta)
 {
         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-        static const u8 baddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+        static const u8 _baddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+        static const u8 *baddr = _baddr;
 
         lockdep_assert_held(&mvm->mutex);
 
+        if (vif->type == NL80211_IFTYPE_ADHOC)
+                baddr = vif->bss_conf.bssid;
+
         if (WARN_ON_ONCE(bsta->sta_id == IWL_MVM_STATION_COUNT))
                 return -ENOSPC;
 
@@ -614,7 +728,7 @@ int iwl_mvm_sta_rx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
                        int tid, u16 ssn, bool start)
 {
         struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
-        struct iwl_mvm_add_sta_cmd cmd = {};
+        struct iwl_mvm_add_sta_cmd_v6 cmd = {};
         int ret;
         u32 status;
 
@@ -638,8 +752,7 @@ int iwl_mvm_sta_rx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
                                   STA_MODIFY_REMOVE_BA_TID;
 
         status = ADD_STA_SUCCESS;
-        ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd),
-                                          &cmd, &status);
+        ret = iwl_mvm_send_add_sta_cmd_status(mvm, &cmd, &status);
         if (ret)
                 return ret;
 
@@ -674,7 +787,7 @@ static int iwl_mvm_sta_tx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
                               int tid, u8 queue, bool start)
 {
         struct iwl_mvm_sta *mvm_sta = (void *)sta->drv_priv;
-        struct iwl_mvm_add_sta_cmd cmd = {};
+        struct iwl_mvm_add_sta_cmd_v6 cmd = {};
         int ret;
         u32 status;
 
@@ -696,8 +809,7 @@ static int iwl_mvm_sta_tx_agg(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
         cmd.tid_disable_tx = cpu_to_le16(mvm_sta->tid_disable_agg);
 
         status = ADD_STA_SUCCESS;
-        ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd),
-                                          &cmd, &status);
+        ret = iwl_mvm_send_add_sta_cmd_status(mvm, &cmd, &status);
         if (ret)
                 return ret;
 
@@ -743,13 +855,13 @@ int iwl_mvm_sta_tx_agg_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
         lockdep_assert_held(&mvm->mutex);
 
-        for (txq_id = IWL_MVM_FIRST_AGG_QUEUE;
-             txq_id <= IWL_MVM_LAST_AGG_QUEUE; txq_id++)
+        for (txq_id = mvm->first_agg_queue;
+             txq_id <= mvm->last_agg_queue; txq_id++)
                 if (mvm->queue_to_mac80211[txq_id] ==
                     IWL_INVALID_MAC80211_QUEUE)
                         break;
 
-        if (txq_id > IWL_MVM_LAST_AGG_QUEUE) {
+        if (txq_id > mvm->last_agg_queue) {
                 IWL_ERR(mvm, "Failed to allocate agg queue\n");
                 return -EIO;
         }
@@ -987,10 +1099,11 @@ static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
                                 u32 cmd_flags)
 {
         __le16 key_flags;
-        struct iwl_mvm_add_sta_cmd cmd = {};
+        struct iwl_mvm_add_sta_key_cmd cmd = {};
         int ret, status;
         u16 keyidx;
         int i;
+        u32 mac_id_n_color = mvm_sta->mac_id_n_color;
 
         keyidx = (keyconf->keyidx << STA_KEY_FLG_KEYID_POS) &
                  STA_KEY_FLG_KEYID_MSK;
@@ -1000,14 +1113,14 @@ static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
         switch (keyconf->cipher) {
         case WLAN_CIPHER_SUITE_TKIP:
                 key_flags |= cpu_to_le16(STA_KEY_FLG_TKIP);
-                cmd.key.tkip_rx_tsc_byte2 = tkip_iv32;
+                cmd.tkip_rx_tsc_byte2 = tkip_iv32;
                 for (i = 0; i < 5; i++)
-                        cmd.key.tkip_rx_ttak[i] = cpu_to_le16(tkip_p1k[i]);
-                memcpy(cmd.key.key, keyconf->key, keyconf->keylen);
+                        cmd.tkip_rx_ttak[i] = cpu_to_le16(tkip_p1k[i]);
+                memcpy(cmd.key, keyconf->key, keyconf->keylen);
                 break;
         case WLAN_CIPHER_SUITE_CCMP:
                 key_flags |= cpu_to_le16(STA_KEY_FLG_CCM);
-                memcpy(cmd.key.key, keyconf->key, keyconf->keylen);
+                memcpy(cmd.key, keyconf->key, keyconf->keylen);
                 break;
         default:
                 WARN_ON(1);
@@ -1017,20 +1130,18 @@ static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
         if (!(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE))
                 key_flags |= cpu_to_le16(STA_KEY_MULTICAST);
 
-        cmd.mac_id_n_color = cpu_to_le32(mvm_sta->mac_id_n_color);
-        cmd.key.key_offset = keyconf->hw_key_idx;
-        cmd.key.key_flags = key_flags;
-        cmd.add_modify = STA_MODE_MODIFY;
-        cmd.modify_mask = STA_MODIFY_KEY;
+        cmd.key_offset = keyconf->hw_key_idx;
+        cmd.key_flags = key_flags;
         cmd.sta_id = sta_id;
 
         status = ADD_STA_SUCCESS;
         if (cmd_flags == CMD_SYNC)
-                ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd),
-                                                  &cmd, &status);
+                ret = iwl_mvm_send_add_sta_key_cmd_status(mvm, &cmd,
+                                                          mac_id_n_color,
+                                                          &status);
         else
-                ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC,
-                                           sizeof(cmd), &cmd);
+                ret = iwl_mvm_send_add_sta_key_cmd(mvm, CMD_ASYNC, &cmd,
+                                                   mac_id_n_color);
 
         switch (status) {
         case ADD_STA_SUCCESS:
@@ -1197,7 +1308,7 @@ int iwl_mvm_remove_sta_key(struct iwl_mvm *mvm,
                            struct ieee80211_key_conf *keyconf)
 {
         struct iwl_mvm_sta *mvm_sta;
-        struct iwl_mvm_add_sta_cmd cmd = {};
+        struct iwl_mvm_add_sta_key_cmd cmd = {};
         __le16 key_flags;
         int ret, status;
         u8 sta_id;
@@ -1252,17 +1363,14 @@ int iwl_mvm_remove_sta_key(struct iwl_mvm *mvm,
         if (!(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE))
                 key_flags |= cpu_to_le16(STA_KEY_MULTICAST);
 
-        cmd.mac_id_n_color = cpu_to_le32(mvm_sta->mac_id_n_color);
-        cmd.key.key_flags = key_flags;
-        cmd.key.key_offset = keyconf->hw_key_idx;
+        cmd.key_flags = key_flags;
+        cmd.key_offset = keyconf->hw_key_idx;
         cmd.sta_id = sta_id;
 
-        cmd.modify_mask = STA_MODIFY_KEY;
-        cmd.add_modify = STA_MODE_MODIFY;
-
         status = ADD_STA_SUCCESS;
-        ret = iwl_mvm_send_cmd_pdu_status(mvm, ADD_STA, sizeof(cmd),
-                                          &cmd, &status);
+        ret = iwl_mvm_send_add_sta_key_cmd_status(mvm, &cmd,
+                                                  mvm_sta->mac_id_n_color,
+                                                  &status);
 
         switch (status) {
         case ADD_STA_SUCCESS:
@@ -1309,7 +1417,7 @@ void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
                                 struct ieee80211_sta *sta)
 {
         struct iwl_mvm_sta *mvmsta = (void *)sta->drv_priv;
-        struct iwl_mvm_add_sta_cmd cmd = {
+        struct iwl_mvm_add_sta_cmd_v6 cmd = {
                 .add_modify = STA_MODE_MODIFY,
                 .sta_id = mvmsta->sta_id,
                 .station_flags_msk = cpu_to_le32(STA_FLG_PS),
@@ -1317,7 +1425,7 @@ void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
         };
         int ret;
 
-        ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC, sizeof(cmd), &cmd);
+        ret = iwl_mvm_send_add_sta_cmd(mvm, CMD_ASYNC, &cmd);
         if (ret)
                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
 }
@@ -1331,7 +1439,7 @@ void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
                 (reason == IEEE80211_FRAME_RELEASE_UAPSD) ?
                         STA_SLEEP_STATE_UAPSD : STA_SLEEP_STATE_PS_POLL;
         struct iwl_mvm_sta *mvmsta = (void *)sta->drv_priv;
-        struct iwl_mvm_add_sta_cmd cmd = {
+        struct iwl_mvm_add_sta_cmd_v6 cmd = {
                 .add_modify = STA_MODE_MODIFY,
                 .sta_id = mvmsta->sta_id,
                 .modify_mask = STA_MODIFY_SLEEPING_STA_TX_COUNT,
@@ -1346,7 +1454,7 @@ void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
         int ret;
 
         /* TODO: somehow the fw doesn't seem to take PS_POLL into account */
-        ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC, sizeof(cmd), &cmd);
+        ret = iwl_mvm_send_add_sta_cmd(mvm, CMD_ASYNC, &cmd);
         if (ret)
                 IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.h b/drivers/net/wireless/iwlwifi/mvm/sta.h
index 94b265eb32b8..4dfc359a4bdd 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.h
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.h
@@ -293,10 +293,6 @@ struct iwl_mvm_sta {
         struct iwl_lq_sta lq_sta;
         struct ieee80211_vif *vif;
 
-#ifdef CONFIG_PM_SLEEP
-        u16 last_seq_ctl;
-#endif
-
         /* Temporary, until the new TLC will control the Tx protection */
         s8 tx_protection;
         bool tt_tx_protection;
diff --git a/drivers/net/wireless/iwlwifi/mvm/testmode.h b/drivers/net/wireless/iwlwifi/mvm/testmode.h
new file mode 100644
index 000000000000..eb74391d91ca
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/mvm/testmode.h
@@ -0,0 +1,95 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2013 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License 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.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2013 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef __IWL_MVM_TESTMODE_H__
+#define __IWL_MVM_TESTMODE_H__
+
+/**
+ * enum iwl_mvm_testmode_attrs - testmode attributes inside NL80211_ATTR_TESTDATA
+ * @IWL_MVM_TM_ATTR_UNSPEC: (invalid attribute)
+ * @IWL_MVM_TM_ATTR_CMD: sub command, see &enum iwl_mvm_testmode_commands (u32)
+ * @IWL_MVM_TM_ATTR_NOA_DURATION: requested NoA duration (u32)
+ * @IWL_MVM_TM_ATTR_BEACON_FILTER_STATE: beacon filter state (0 or 1, u32)
+ */
+enum iwl_mvm_testmode_attrs {
+        IWL_MVM_TM_ATTR_UNSPEC,
+        IWL_MVM_TM_ATTR_CMD,
+        IWL_MVM_TM_ATTR_NOA_DURATION,
+        IWL_MVM_TM_ATTR_BEACON_FILTER_STATE,
+
+        /* keep last */
+        NUM_IWL_MVM_TM_ATTRS,
+        IWL_MVM_TM_ATTR_MAX = NUM_IWL_MVM_TM_ATTRS - 1,
+};
+
+/**
+ * enum iwl_mvm_testmode_commands - MVM testmode commands
+ * @IWL_MVM_TM_CMD_SET_NOA: set NoA on GO vif for testing
+ * @IWL_MVM_TM_CMD_SET_BEACON_FILTER: turn beacon filtering off/on
+ */
+enum iwl_mvm_testmode_commands {
+        IWL_MVM_TM_CMD_SET_NOA,
+        IWL_MVM_TM_CMD_SET_BEACON_FILTER,
+};
+
+#endif /* __IWL_MVM_TESTMODE_H__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/time-event.c b/drivers/net/wireless/iwlwifi/mvm/time-event.c
index 76a3c177e100..33cf56fdfc41 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.c
+++ b/drivers/net/wireless/iwlwifi/mvm/time-event.c
@@ -387,7 +387,8 @@ static int iwl_mvm_time_event_send_add(struct iwl_mvm *mvm,
 
 void iwl_mvm_protect_session(struct iwl_mvm *mvm,
                              struct ieee80211_vif *vif,
-                             u32 duration, u32 min_duration)
+                             u32 duration, u32 min_duration,
+                             u32 max_delay)
 {
         struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
         struct iwl_mvm_time_event_data *te_data = &mvmvif->time_event_data;
@@ -426,7 +427,7 @@ void iwl_mvm_protect_session(struct iwl_mvm *mvm,
                 cpu_to_le32(iwl_read_prph(mvm->trans, DEVICE_SYSTEM_TIME_REG));
 
         time_cmd.max_frags = TE_V2_FRAG_NONE;
-        time_cmd.max_delay = cpu_to_le32(500);
+        time_cmd.max_delay = cpu_to_le32(max_delay);
         /* TODO: why do we need to interval = bi if it is not periodic? */
         time_cmd.interval = cpu_to_le32(1);
         time_cmd.duration = cpu_to_le32(duration);
diff --git a/drivers/net/wireless/iwlwifi/mvm/time-event.h b/drivers/net/wireless/iwlwifi/mvm/time-event.h
index f86c51065ed3..d9c8d6cfa2db 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.h
+++ b/drivers/net/wireless/iwlwifi/mvm/time-event.h
@@ -123,6 +123,7 @@
  * @duration: the duration of the session in TU.
  * @min_duration: will start a new session if the current session will end
  *      in less than min_duration.
+ * @max_delay: maximum delay before starting the time event (in TU)
  *
  * This function can be used to start a session protection which means that the
  * fw will stay on the channel for %duration_ms milliseconds. This function
@@ -133,7 +134,8 @@
  */
 void iwl_mvm_protect_session(struct iwl_mvm *mvm,
                              struct ieee80211_vif *vif,
-                             u32 duration, u32 min_duration);
+                             u32 duration, u32 min_duration,
+                             u32 max_delay);
 
 /**
  * iwl_mvm_stop_session_protection - cancel the session protection.
diff --git a/drivers/net/wireless/iwlwifi/mvm/tx.c b/drivers/net/wireless/iwlwifi/mvm/tx.c
index e05440d90319..43d97c33a75a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/tx.c
@@ -417,7 +417,7 @@ int iwl_mvm_tx_skb(struct iwl_mvm *mvm, struct sk_buff *skb,
 
         spin_unlock(&mvmsta->lock);
 
-        if (txq_id < IWL_MVM_FIRST_AGG_QUEUE)
+        if (txq_id < mvm->first_agg_queue)
                 atomic_inc(&mvm->pending_frames[mvmsta->sta_id]);
 
         return 0;
@@ -511,16 +511,10 @@ const char *iwl_mvm_get_tx_fail_reason(u32 status)
 }
 #endif /* CONFIG_IWLWIFI_DEBUG */
 
-/**
- * translate ucode response to mac80211 tx status control values
- */
-static void iwl_mvm_hwrate_to_tx_control(u32 rate_n_flags,
-                                         struct ieee80211_tx_info *info)
+void iwl_mvm_hwrate_to_tx_rate(u32 rate_n_flags,
+                               enum ieee80211_band band,
+                               struct ieee80211_tx_rate *r)
 {
-        struct ieee80211_tx_rate *r = &info->status.rates[0];
-
-        info->status.antenna =
-                ((rate_n_flags & RATE_MCS_ANT_ABC_MSK) >> RATE_MCS_ANT_POS);
         if (rate_n_flags & RATE_HT_MCS_GF_MSK)
                 r->flags |= IEEE80211_TX_RC_GREEN_FIELD;
         switch (rate_n_flags & RATE_MCS_CHAN_WIDTH_MSK) {
@@ -549,10 +543,23 @@ static void iwl_mvm_hwrate_to_tx_control(u32 rate_n_flags,
                 r->flags |= IEEE80211_TX_RC_VHT_MCS;
         } else {
                 r->idx = iwl_mvm_legacy_rate_to_mac80211_idx(rate_n_flags,
-                                                             info->band);
+                                                             band);
         }
 }
 
+/**
+ * translate ucode response to mac80211 tx status control values
+ */
+static void iwl_mvm_hwrate_to_tx_status(u32 rate_n_flags,
+                                        struct ieee80211_tx_info *info)
+{
+        struct ieee80211_tx_rate *r = &info->status.rates[0];
+
+        info->status.antenna =
+                ((rate_n_flags & RATE_MCS_ANT_ABC_MSK) >> RATE_MCS_ANT_POS);
+        iwl_mvm_hwrate_to_tx_rate(rate_n_flags, info->band, r);
+}
+
 static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
                                      struct iwl_rx_packet *pkt)
 {
@@ -602,11 +609,11 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
                 }
 
                 info->status.rates[0].count = tx_resp->failure_frame + 1;
-                iwl_mvm_hwrate_to_tx_control(le32_to_cpu(tx_resp->initial_rate),
-                                             info);
+                iwl_mvm_hwrate_to_tx_status(le32_to_cpu(tx_resp->initial_rate),
+                                            info);
 
                 /* Single frame failure in an AMPDU queue => send BAR */
-                if (txq_id >= IWL_MVM_FIRST_AGG_QUEUE &&
+                if (txq_id >= mvm->first_agg_queue &&
                     !(info->flags & IEEE80211_TX_STAT_ACK))
                         info->flags |= IEEE80211_TX_STAT_AMPDU_NO_BACK;
 
@@ -619,7 +626,7 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
                 ieee80211_tx_status_ni(mvm->hw, skb);
         }
 
-        if (txq_id >= IWL_MVM_FIRST_AGG_QUEUE) {
+        if (txq_id >= mvm->first_agg_queue) {
                 /* If this is an aggregation queue, we use the ssn since:
                  * ssn = wifi seq_num % 256.
                  * The seq_ctl is the sequence control of the packet to which
@@ -668,10 +675,6 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
                         iwl_mvm_check_ratid_empty(mvm, sta, tid);
                         spin_unlock_bh(&mvmsta->lock);
                 }
-
-#ifdef CONFIG_PM_SLEEP
-                mvmsta->last_seq_ctl = seq_ctl;
-#endif
         } else {
                 sta = NULL;
                 mvmsta = NULL;
@@ -681,7 +684,7 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
          * If the txq is not an AMPDU queue, there is no chance we freed
          * several skbs. Check that out...
          */
-        if (txq_id < IWL_MVM_FIRST_AGG_QUEUE && !WARN_ON(skb_freed > 1) &&
+        if (txq_id < mvm->first_agg_queue && !WARN_ON(skb_freed > 1) &&
             atomic_sub_and_test(skb_freed, &mvm->pending_frames[sta_id])) {
                 if (mvmsta) {
                         /*
@@ -777,7 +780,7 @@ static void iwl_mvm_rx_tx_cmd_agg(struct iwl_mvm *mvm,
         u16 sequence = le16_to_cpu(pkt->hdr.sequence);
         struct ieee80211_sta *sta;
 
-        if (WARN_ON_ONCE(SEQ_TO_QUEUE(sequence) < IWL_MVM_FIRST_AGG_QUEUE))
+        if (WARN_ON_ONCE(SEQ_TO_QUEUE(sequence) < mvm->first_agg_queue))
                 return;
 
         if (WARN_ON_ONCE(tid == IWL_TID_NON_QOS))
@@ -904,8 +907,8 @@ int iwl_mvm_rx_ba_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
                         info->flags |= IEEE80211_TX_STAT_AMPDU;
                         info->status.ampdu_ack_len = ba_notif->txed_2_done;
                         info->status.ampdu_len = ba_notif->txed;
-                        iwl_mvm_hwrate_to_tx_control(tid_data->rate_n_flags,
-                                                     info);
+                        iwl_mvm_hwrate_to_tx_status(tid_data->rate_n_flags,
+                                                    info);
                 }
         }
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/utils.c b/drivers/net/wireless/iwlwifi/mvm/utils.c
index a9c357491434..ed69e9b78e82 100644
--- a/drivers/net/wireless/iwlwifi/mvm/utils.c
+++ b/drivers/net/wireless/iwlwifi/mvm/utils.c
@@ -466,7 +466,7 @@ void iwl_mvm_dump_sram(struct iwl_mvm *mvm)
         ofs = img->sec[IWL_UCODE_SECTION_DATA].offset;
         len = img->sec[IWL_UCODE_SECTION_DATA].len;
 
-        buf = kzalloc(len, GFP_KERNEL);
+        buf = kzalloc(len, GFP_ATOMIC);
         if (!buf)
                 return;
 
diff --git a/drivers/net/wireless/iwlwifi/pcie/drv.c b/drivers/net/wireless/iwlwifi/pcie/drv.c
index 26108a1a29fa..941c0c88f982 100644
--- a/drivers/net/wireless/iwlwifi/pcie/drv.c
+++ b/drivers/net/wireless/iwlwifi/pcie/drv.c
@@ -268,7 +268,7 @@ static DEFINE_PCI_DEVICE_TABLE(iwl_hw_card_ids) = {
 #endif /* CONFIG_IWLDVM */
 
 #if IS_ENABLED(CONFIG_IWLMVM)
-/* 7000 Series */
+/* 7260 Series */
         {IWL_PCI_DEVICE(0x08B1, 0x4070, iwl7260_2ac_cfg)},
         {IWL_PCI_DEVICE(0x08B1, 0x4072, iwl7260_2ac_cfg)},
         {IWL_PCI_DEVICE(0x08B1, 0x4170, iwl7260_2ac_cfg)},
@@ -350,6 +350,9 @@ static DEFINE_PCI_DEVICE_TABLE(iwl_hw_card_ids) = {
         {IWL_PCI_DEVICE(0x08B4, 0x8270, iwl3160_2ac_cfg)},
         {IWL_PCI_DEVICE(0x08B3, 0x8470, iwl3160_2ac_cfg)},
         {IWL_PCI_DEVICE(0x08B3, 0x8570, iwl3160_2ac_cfg)},
+
+/* 7265 Series */
+        {IWL_PCI_DEVICE(0x095A, 0x5010, iwl7265_2ac_cfg)},
 #endif /* CONFIG_IWLMVM */
 
         {0}
@@ -391,7 +394,6 @@ out_free_drv:
         iwl_drv_stop(trans_pcie->drv);
 out_free_trans:
         iwl_trans_pcie_free(iwl_trans);
-        pci_set_drvdata(pdev, NULL);
         return ret;
 }
 
@@ -402,8 +404,6 @@ static void iwl_pci_remove(struct pci_dev *pdev)
 
         iwl_drv_stop(trans_pcie->drv);
         iwl_trans_pcie_free(trans);
-
-        pci_set_drvdata(pdev, NULL);
 }
 
 #ifdef CONFIG_PM_SLEEP
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index c3f904d422b0..5d9337bec67a 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -220,6 +220,9 @@ static int iwl_pcie_apm_init(struct iwl_trans *trans)
         iwl_set_bits_prph(trans, APMG_PCIDEV_STT_REG,
                           APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 
+        /* Clear the interrupt in APMG if the NIC is in RFKILL */
+        iwl_write_prph(trans, APMG_RTC_INT_STT_REG, APMG_RTC_INT_STT_RFKILL);
+
         set_bit(STATUS_DEVICE_ENABLED, &trans_pcie->status);
 
 out:
@@ -443,22 +446,138 @@ static int iwl_pcie_load_section(struct iwl_trans *trans, u8 section_num,
         return ret;
 }
 
+static int iwl_pcie_secure_set(struct iwl_trans *trans, int cpu)
+{
+        int shift_param;
+        u32 address;
+        int ret = 0;
+
+        if (cpu == 1) {
+                shift_param = 0;
+                address = CSR_SECURE_BOOT_CPU1_STATUS_ADDR;
+        } else {
+                shift_param = 16;
+                address = CSR_SECURE_BOOT_CPU2_STATUS_ADDR;
+        }
+
+        /* set CPU to started */
+        iwl_trans_set_bits_mask(trans,
+                                CSR_UCODE_LOAD_STATUS_ADDR,
+                                CSR_CPU_STATUS_LOADING_STARTED << shift_param,
+                                1);
+
+        /* set last complete descriptor number */
+        iwl_trans_set_bits_mask(trans,
+                                CSR_UCODE_LOAD_STATUS_ADDR,
+                                CSR_CPU_STATUS_NUM_OF_LAST_COMPLETED
+                                << shift_param,
+                                1);
+
+        /* set last loaded block */
+        iwl_trans_set_bits_mask(trans,
+                                CSR_UCODE_LOAD_STATUS_ADDR,
+                                CSR_CPU_STATUS_NUM_OF_LAST_LOADED_BLOCK
+                                << shift_param,
+                                1);
+
+        /* image loading complete */
+        iwl_trans_set_bits_mask(trans,
+                                CSR_UCODE_LOAD_STATUS_ADDR,
+                                CSR_CPU_STATUS_LOADING_COMPLETED
+                                << shift_param,
+                                1);
+
+        /* set FH_TCSR_0_REG  */
+        iwl_trans_set_bits_mask(trans, FH_TCSR_0_REG0, 0x00400000, 1);
+
+        /* verify image verification started  */
+        ret = iwl_poll_bit(trans, address,
+                           CSR_SECURE_BOOT_CPU_STATUS_VERF_STATUS,
+                           CSR_SECURE_BOOT_CPU_STATUS_VERF_STATUS,
+                           CSR_SECURE_TIME_OUT);
+        if (ret < 0) {
+                IWL_ERR(trans, "secure boot process didn't start\n");
+                return ret;
+        }
+
+        /* wait for image verification to complete  */
+        ret = iwl_poll_bit(trans, address,
+                           CSR_SECURE_BOOT_CPU_STATUS_VERF_COMPLETED,
+                           CSR_SECURE_BOOT_CPU_STATUS_VERF_COMPLETED,
+                           CSR_SECURE_TIME_OUT);
+
+        if (ret < 0) {
+                IWL_ERR(trans, "Time out on secure boot process\n");
+                return ret;
+        }
+
+        return 0;
+}
+
 static int iwl_pcie_load_given_ucode(struct iwl_trans *trans,
                                 const struct fw_img *image)
 {
         int i, ret = 0;
 
-        for (i = 0; i < IWL_UCODE_SECTION_MAX; i++) {
+        IWL_DEBUG_FW(trans,
+                     "working with %s image\n",
+                     image->is_secure ? "Secured" : "Non Secured");
+        IWL_DEBUG_FW(trans,
+                     "working with %s CPU\n",
+                     image->is_dual_cpus ? "Dual" : "Single");
+
+        /* configure the ucode to be ready to get the secured image */
+        if (image->is_secure) {
+                /* set secure boot inspector addresses */
+                iwl_write32(trans, CSR_SECURE_INSPECTOR_CODE_ADDR, 0);
+                iwl_write32(trans, CSR_SECURE_INSPECTOR_DATA_ADDR, 0);
+
+                /* release CPU1 reset if secure inspector image burned in OTP */
+                iwl_write32(trans, CSR_RESET, 0);
+        }
+
+        /* load to FW the binary sections of CPU1 */
+        IWL_DEBUG_INFO(trans, "Loading CPU1\n");
+        for (i = 0;
+             i < IWL_UCODE_FIRST_SECTION_OF_SECOND_CPU;
+             i++) {
                 if (!image->sec[i].data)
                         break;
-
                 ret = iwl_pcie_load_section(trans, i, &image->sec[i]);
                 if (ret)
                         return ret;
         }
 
-        /* Remove all resets to allow NIC to operate */
-        iwl_write32(trans, CSR_RESET, 0);
+        /* configure the ucode to start secure process on CPU1 */
+        if (image->is_secure) {
+                /* config CPU1 to start secure protocol */
+                ret = iwl_pcie_secure_set(trans, 1);
+                if (ret)
+                        return ret;
+        } else {
+                /* Remove all resets to allow NIC to operate */
+                iwl_write32(trans, CSR_RESET, 0);
+        }
+
+        if (image->is_dual_cpus) {
+                /* load to FW the binary sections of CPU2 */
+                IWL_DEBUG_INFO(trans, "working w/ DUAL CPUs - Loading CPU2\n");
+                for (i = IWL_UCODE_FIRST_SECTION_OF_SECOND_CPU;
+                        i < IWL_UCODE_SECTION_MAX; i++) {
+                        if (!image->sec[i].data)
+                                break;
+                        ret = iwl_pcie_load_section(trans, i, &image->sec[i]);
+                        if (ret)
+                                return ret;
+                }
+
+                if (image->is_secure) {
+                        /* set CPU2 for secure protocol */
+                        ret = iwl_pcie_secure_set(trans, 2);
+                        if (ret)
+                                return ret;
+                }
+        }
 
         return 0;
 }
diff --git a/drivers/net/wireless/iwlwifi/pcie/tx.c b/drivers/net/wireless/iwlwifi/pcie/tx.c
index 1424335163b9..f644fcf861a8 100644
--- a/drivers/net/wireless/iwlwifi/pcie/tx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/tx.c
@@ -1465,7 +1465,8 @@ void iwl_pcie_hcmd_complete(struct iwl_trans *trans,
         spin_unlock_bh(&txq->lock);
 }
 
-#define HOST_COMPLETE_TIMEOUT (2 * HZ)
+#define HOST_COMPLETE_TIMEOUT   (2 * HZ)
+#define COMMAND_POKE_TIMEOUT    (HZ / 10)
 
 static int iwl_pcie_send_hcmd_async(struct iwl_trans *trans,
                                     struct iwl_host_cmd *cmd)
@@ -1493,6 +1494,7 @@ static int iwl_pcie_send_hcmd_sync(struct iwl_trans *trans,
         struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
         int cmd_idx;
         int ret;
+        int timeout = HOST_COMPLETE_TIMEOUT;
 
         IWL_DEBUG_INFO(trans, "Attempting to send sync command %s\n",
                        get_cmd_string(trans_pcie, cmd->id));
@@ -1517,10 +1519,29 @@ static int iwl_pcie_send_hcmd_sync(struct iwl_trans *trans,
                 return ret;
         }
 
-        ret = wait_event_timeout(trans_pcie->wait_command_queue,
-                                 !test_bit(STATUS_HCMD_ACTIVE,
-                                           &trans_pcie->status),
-                                 HOST_COMPLETE_TIMEOUT);
+        while (timeout > 0) {
+                unsigned long flags;
+
+                timeout -= COMMAND_POKE_TIMEOUT;
+                ret = wait_event_timeout(trans_pcie->wait_command_queue,
+                                         !test_bit(STATUS_HCMD_ACTIVE,
+                                                   &trans_pcie->status),
+                                         COMMAND_POKE_TIMEOUT);
+                if (ret)
+                        break;
+                /* poke the device - it may have lost the command */
+                if (iwl_trans_grab_nic_access(trans, true, &flags)) {
+                        iwl_trans_release_nic_access(trans, &flags);
+                        IWL_DEBUG_INFO(trans,
+                                       "Tried to wake NIC for command %s\n",
+                                       get_cmd_string(trans_pcie, cmd->id));
+                } else {
+                        IWL_ERR(trans, "Failed to poke NIC for command %s\n",
+                                get_cmd_string(trans_pcie, cmd->id));
+                        break;
+                }
+        }
+
         if (!ret) {
                 if (test_bit(STATUS_HCMD_ACTIVE, &trans_pcie->status)) {
                         struct iwl_txq *txq =
@@ -1541,6 +1562,9 @@ static int iwl_pcie_send_hcmd_sync(struct iwl_trans *trans,
                                        "Clearing HCMD_ACTIVE for command %s\n",
                                        get_cmd_string(trans_pcie, cmd->id));
                         ret = -ETIMEDOUT;
+
+                        iwl_op_mode_nic_error(trans->op_mode);
+
                         goto cancel;
                 }
         }
diff --git a/drivers/net/wireless/libertas/firmware.c b/drivers/net/wireless/libertas/firmware.c
index c0f9e7e862f6..51b92b5df119 100644
--- a/drivers/net/wireless/libertas/firmware.c
+++ b/drivers/net/wireless/libertas/firmware.c
@@ -53,6 +53,11 @@ static void main_firmware_cb(const struct firmware *firmware, void *context)
 
         /* Firmware found! */
         lbs_fw_loaded(priv, 0, priv->helper_fw, firmware);
+        if (priv->helper_fw) {
+                release_firmware (priv->helper_fw);
+                priv->helper_fw = NULL;
+        }
+        release_firmware (firmware);
 }
 
 static void helper_firmware_cb(const struct firmware *firmware, void *context)
diff --git a/drivers/net/wireless/libertas/if_cs.c b/drivers/net/wireless/libertas/if_cs.c
index c94dd6802672..ef8c98e21098 100644
--- a/drivers/net/wireless/libertas/if_cs.c
+++ b/drivers/net/wireless/libertas/if_cs.c
@@ -754,14 +754,14 @@ static void if_cs_prog_firmware(struct lbs_private *priv, int ret,
         if (ret == 0 && (card->model != MODEL_8305))
                 ret = if_cs_prog_real(card, mainfw);
         if (ret)
-                goto out;
+                return;
 
         /* Now actually get the IRQ */
         ret = request_irq(card->p_dev->irq, if_cs_interrupt,
                 IRQF_SHARED, DRV_NAME, card);
         if (ret) {
                 pr_err("error in request_irq\n");
-                goto out;
+                return;
         }
 
         /*
@@ -777,10 +777,6 @@ static void if_cs_prog_firmware(struct lbs_private *priv, int ret,
                 pr_err("could not activate card\n");
                 free_irq(card->p_dev->irq, card);
         }
-
-out:
-        release_firmware(helper);
-        release_firmware(mainfw);
 }
 
 
diff --git a/drivers/net/wireless/libertas/if_sdio.c b/drivers/net/wireless/libertas/if_sdio.c
index 45578335e420..991238afd1b6 100644
--- a/drivers/net/wireless/libertas/if_sdio.c
+++ b/drivers/net/wireless/libertas/if_sdio.c
@@ -708,20 +708,16 @@ static void if_sdio_do_prog_firmware(struct lbs_private *priv, int ret,
 
         ret = if_sdio_prog_helper(card, helper);
         if (ret)
-                goto out;
+                return;
 
         lbs_deb_sdio("Helper firmware loaded\n");
 
         ret = if_sdio_prog_real(card, mainfw);
         if (ret)
-                goto out;
+                return;
 
         lbs_deb_sdio("Firmware loaded\n");
         if_sdio_finish_power_on(card);
-
-out:
-        release_firmware(helper);
-        release_firmware(mainfw);
 }
 
 static int if_sdio_prog_firmware(struct if_sdio_card *card)
diff --git a/drivers/net/wireless/libertas/if_spi.c b/drivers/net/wireless/libertas/if_spi.c
index 5d39ec880d84..83669151bb82 100644
--- a/drivers/net/wireless/libertas/if_spi.c
+++ b/drivers/net/wireless/libertas/if_spi.c
@@ -1094,11 +1094,7 @@ static int if_spi_init_card(struct if_spi_card *card)
                 goto out;
 
 out:
-        release_firmware(helper);
-        release_firmware(mainfw);
-
         lbs_deb_leave_args(LBS_DEB_SPI, "err %d\n", err);
-
         return err;
 }
 
diff --git a/drivers/net/wireless/libertas/if_usb.c b/drivers/net/wireless/libertas/if_usb.c
index 27980778d992..dff08a2896a3 100644
--- a/drivers/net/wireless/libertas/if_usb.c
+++ b/drivers/net/wireless/libertas/if_usb.c
@@ -844,7 +844,7 @@ static void if_usb_prog_firmware(struct lbs_private *priv, int ret,
         cardp->fw = fw;
         if (check_fwfile_format(cardp->fw->data, cardp->fw->size)) {
                 ret = -EINVAL;
-                goto release_fw;
+                goto done;
         }
 
         /* Cancel any pending usb business */
@@ -861,7 +861,7 @@ restart:
         if (if_usb_submit_rx_urb_fwload(cardp) < 0) {
                 lbs_deb_usbd(&cardp->udev->dev, "URB submission is failed\n");
                 ret = -EIO;
-                goto release_fw;
+                goto done;
         }
 
         cardp->bootcmdresp = 0;
@@ -883,14 +883,14 @@ restart:
                 usb_kill_urb(cardp->tx_urb);
                 if (if_usb_submit_rx_urb(cardp) < 0)
                         ret = -EIO;
-                goto release_fw;
+                goto done;
         } else if (cardp->bootcmdresp <= 0) {
                 if (--reset_count >= 0) {
                         if_usb_reset_device(cardp);
                         goto restart;
                 }
                 ret = -EIO;
-                goto release_fw;
+                goto done;
         }
 
         i = 0;
@@ -921,14 +921,14 @@ restart:
 
                 pr_info("FW download failure, time = %d ms\n", i * 100);
                 ret = -EIO;
-                goto release_fw;
+                goto done;
         }
 
         cardp->priv->fw_ready = 1;
         if_usb_submit_rx_urb(cardp);
 
         if (lbs_start_card(priv))
-                goto release_fw;
+                goto done;
 
         if_usb_setup_firmware(priv);
 
@@ -939,11 +939,8 @@ restart:
         if (lbs_host_sleep_cfg(priv, priv->wol_criteria, NULL))
                 priv->ehs_remove_supported = false;
 
- release_fw:
-        release_firmware(cardp->fw);
-        cardp->fw = NULL;
-
  done:
+        cardp->fw = NULL;
         lbs_deb_leave(LBS_DEB_USB);
 }
 
diff --git a/drivers/net/wireless/rt2x00/Kconfig b/drivers/net/wireless/rt2x00/Kconfig
index a18b0051a745..006b8bcb2e31 100644
--- a/drivers/net/wireless/rt2x00/Kconfig
+++ b/drivers/net/wireless/rt2x00/Kconfig
@@ -58,11 +58,11 @@ config RT61PCI
 
 config RT2800PCI
         tristate "Ralink rt27xx/rt28xx/rt30xx (PCI/PCIe/PCMCIA) support"
-        depends on PCI || SOC_RT288X || SOC_RT305X
+        depends on PCI
         select RT2800_LIB
+        select RT2800_LIB_MMIO
         select RT2X00_LIB_MMIO
-        select RT2X00_LIB_PCI if PCI
-        select RT2X00_LIB_SOC if SOC_RT288X || SOC_RT305X
+        select RT2X00_LIB_PCI
         select RT2X00_LIB_FIRMWARE
         select RT2X00_LIB_CRYPTO
         select CRC_CCITT
@@ -199,9 +199,30 @@ config RT2800USB_UNKNOWN
 
 endif
 
+config RT2800SOC
+        tristate "Ralink WiSoC support"
+        depends on SOC_RT288X || SOC_RT305X
+        select RT2X00_LIB_SOC
+        select RT2X00_LIB_MMIO
+        select RT2X00_LIB_CRYPTO
+        select RT2X00_LIB_FIRMWARE
+        select RT2800_LIB
+        select RT2800_LIB_MMIO
+        ---help---
+          This adds support for Ralink WiSoC devices.
+          Supported chips: RT2880, RT3050, RT3052, RT3350, RT3352.
+
+          When compiled as a module, this driver will be called rt2800soc.
+
+
 config RT2800_LIB
         tristate
 
+config RT2800_LIB_MMIO
+        tristate
+        select RT2X00_LIB_MMIO
+        select RT2800_LIB
+
 config RT2X00_LIB_MMIO
         tristate
 
diff --git a/drivers/net/wireless/rt2x00/Makefile b/drivers/net/wireless/rt2x00/Makefile
index f069d8bc5b67..24a66015a495 100644
--- a/drivers/net/wireless/rt2x00/Makefile
+++ b/drivers/net/wireless/rt2x00/Makefile
@@ -14,6 +14,7 @@ obj-$(CONFIG_RT2X00_LIB_PCI)		+= rt2x00pci.o
 obj-$(CONFIG_RT2X00_LIB_SOC)            += rt2x00soc.o
 obj-$(CONFIG_RT2X00_LIB_USB)            += rt2x00usb.o
 obj-$(CONFIG_RT2800_LIB)                += rt2800lib.o
+obj-$(CONFIG_RT2800_LIB_MMIO)           += rt2800mmio.o
 obj-$(CONFIG_RT2400PCI)                 += rt2400pci.o
 obj-$(CONFIG_RT2500PCI)                 += rt2500pci.o
 obj-$(CONFIG_RT61PCI)                   += rt61pci.o
@@ -21,3 +22,4 @@ obj-$(CONFIG_RT2800PCI)			+= rt2800pci.o
 obj-$(CONFIG_RT2500USB)                 += rt2500usb.o
 obj-$(CONFIG_RT73USB)                   += rt73usb.o
 obj-$(CONFIG_RT2800USB)                 += rt2800usb.o
+obj-$(CONFIG_RT2800SOC)                 += rt2800soc.o
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index 3d53a09da5a1..38ed9a3e44c8 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -1261,7 +1261,7 @@ static void rt2400pci_fill_rxdone(struct queue_entry *entry,
          */
         rxdesc->timestamp = ((u64)rx_high << 32) | rx_low;
         rxdesc->signal = rt2x00_get_field32(word2, RXD_W2_SIGNAL) & ~0x08;
-        rxdesc->rssi = rt2x00_get_field32(word2, RXD_W3_RSSI) -
+        rxdesc->rssi = rt2x00_get_field32(word3, RXD_W3_RSSI) -
             entry->queue->rt2x00dev->rssi_offset;
         rxdesc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
 
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index aa8789423937..c5738f14c4ba 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -7450,7 +7450,6 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
         char *default_power2;
         char *default_power3;
         unsigned int i;
-        u16 eeprom;
         u32 reg;
 
         /*
@@ -7499,46 +7498,48 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
         rt2x00dev->hw->max_report_rates = 7;
         rt2x00dev->hw->max_rate_tries = 1;
 
-        rt2800_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
-
         /*
          * Initialize hw_mode information.
          */
-        spec->supported_bands = SUPPORT_BAND_2GHZ;
         spec->supported_rates = SUPPORT_RATE_CCK | SUPPORT_RATE_OFDM;
 
-        if (rt2x00_rf(rt2x00dev, RF2820) ||
-            rt2x00_rf(rt2x00dev, RF2720)) {
+        switch (rt2x00dev->chip.rf) {
+        case RF2720:
+        case RF2820:
                 spec->num_channels = 14;
                 spec->channels = rf_vals;
-        } else if (rt2x00_rf(rt2x00dev, RF2850) ||
-                   rt2x00_rf(rt2x00dev, RF2750)) {
-                spec->supported_bands |= SUPPORT_BAND_5GHZ;
+                break;
+
+        case RF2750:
+        case RF2850:
                 spec->num_channels = ARRAY_SIZE(rf_vals);
                 spec->channels = rf_vals;
-        } else if (rt2x00_rf(rt2x00dev, RF3020) ||
-                   rt2x00_rf(rt2x00dev, RF2020) ||
-                   rt2x00_rf(rt2x00dev, RF3021) ||
-                   rt2x00_rf(rt2x00dev, RF3022) ||
-                   rt2x00_rf(rt2x00dev, RF3070) ||
-                   rt2x00_rf(rt2x00dev, RF3290) ||
-                   rt2x00_rf(rt2x00dev, RF3320) ||
-                   rt2x00_rf(rt2x00dev, RF3322) ||
-                   rt2x00_rf(rt2x00dev, RF5360) ||
-                   rt2x00_rf(rt2x00dev, RF5370) ||
-                   rt2x00_rf(rt2x00dev, RF5372) ||
-                   rt2x00_rf(rt2x00dev, RF5390) ||
-                   rt2x00_rf(rt2x00dev, RF5392)) {
+                break;
+
+        case RF2020:
+        case RF3020:
+        case RF3021:
+        case RF3022:
+        case RF3070:
+        case RF3290:
+        case RF3320:
+        case RF3322:
+        case RF5360:
+        case RF5370:
+        case RF5372:
+        case RF5390:
+        case RF5392:
                 spec->num_channels = 14;
                 spec->channels = rf_vals_3x;
-        } else if (rt2x00_rf(rt2x00dev, RF3052) ||
-                   rt2x00_rf(rt2x00dev, RF3053)) {
-                spec->supported_bands |= SUPPORT_BAND_5GHZ;
+                break;
+
+        case RF3052:
+        case RF3053:
                 spec->num_channels = ARRAY_SIZE(rf_vals_3x);
                 spec->channels = rf_vals_3x;
-        } else if (rt2x00_rf(rt2x00dev, RF5592)) {
-                spec->supported_bands |= SUPPORT_BAND_5GHZ;
+                break;
 
+        case RF5592:
                 rt2800_register_read(rt2x00dev, MAC_DEBUG_INDEX, &reg);
                 if (rt2x00_get_field32(reg, MAC_DEBUG_INDEX_XTAL)) {
                         spec->num_channels = ARRAY_SIZE(rf_vals_5592_xtal40);
@@ -7547,11 +7548,16 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
                         spec->num_channels = ARRAY_SIZE(rf_vals_5592_xtal20);
                         spec->channels = rf_vals_5592_xtal20;
                 }
+                break;
         }
 
         if (WARN_ON_ONCE(!spec->channels))
                 return -ENODEV;
 
+        spec->supported_bands = SUPPORT_BAND_2GHZ;
+        if (spec->num_channels > 14)
+                spec->supported_bands |= SUPPORT_BAND_5GHZ;
+
         /*
          * Initialize HT information.
          */
@@ -7566,22 +7572,21 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
             IEEE80211_HT_CAP_SGI_20 |
             IEEE80211_HT_CAP_SGI_40;
 
-        if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_TXPATH) >= 2)
+        if (rt2x00dev->default_ant.tx_chain_num >= 2)
                 spec->ht.cap |= IEEE80211_HT_CAP_TX_STBC;
 
-        spec->ht.cap |=
-            rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH) <<
-                IEEE80211_HT_CAP_RX_STBC_SHIFT;
+        spec->ht.cap |= rt2x00dev->default_ant.rx_chain_num <<
+                        IEEE80211_HT_CAP_RX_STBC_SHIFT;
 
         spec->ht.ampdu_factor = 3;
         spec->ht.ampdu_density = 4;
         spec->ht.mcs.tx_params =
             IEEE80211_HT_MCS_TX_DEFINED |
             IEEE80211_HT_MCS_TX_RX_DIFF |
-            ((rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_TXPATH) - 1) <<
-                IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT);
+            ((rt2x00dev->default_ant.tx_chain_num - 1) <<
+             IEEE80211_HT_MCS_TX_MAX_STREAMS_SHIFT);
 
-        switch (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH)) {
+        switch (rt2x00dev->default_ant.rx_chain_num) {
         case 3:
                 spec->ht.mcs.rx_mask[2] = 0xff;
         case 2:
diff --git a/drivers/net/wireless/rt2x00/rt2800mmio.c b/drivers/net/wireless/rt2x00/rt2800mmio.c
new file mode 100644
index 000000000000..ae152280e071
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2800mmio.c
@@ -0,0 +1,873 @@
+/*      Copyright (C) 2009 - 2010 Ivo van Doorn <IvDoorn@gmail.com>
+ *      Copyright (C) 2009 Alban Browaeys <prahal@yahoo.com>
+ *      Copyright (C) 2009 Felix Fietkau <nbd@openwrt.org>
+ *      Copyright (C) 2009 Luis Correia <luis.f.correia@gmail.com>
+ *      Copyright (C) 2009 Mattias Nissler <mattias.nissler@gmx.de>
+ *      Copyright (C) 2009 Mark Asselstine <asselsm@gmail.com>
+ *      Copyright (C) 2009 Xose Vazquez Perez <xose.vazquez@gmail.com>
+ *      Copyright (C) 2009 Bart Zolnierkiewicz <bzolnier@gmail.com>
+ *      <http://rt2x00.serialmonkey.com>
+ *
+ *      This program is free software; you can redistribute it and/or modify
+ *      it under the terms of the GNU General Public License as published by
+ *      the Free Software Foundation; either version 2 of the License, or
+ *      (at your option) any later version.
+ *
+ *      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.
+ *
+ *      You should have received a copy of the GNU General Public License
+ *      along with this program; if not, write to the
+ *      Free Software Foundation, Inc.,
+ *      59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*      Module: rt2800mmio
+ *      Abstract: rt2800 MMIO device routines.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/export.h>
+
+#include "rt2x00.h"
+#include "rt2x00mmio.h"
+#include "rt2800.h"
+#include "rt2800lib.h"
+#include "rt2800mmio.h"
+
+/*
+ * TX descriptor initialization
+ */
+__le32 *rt2800mmio_get_txwi(struct queue_entry *entry)
+{
+        return (__le32 *) entry->skb->data;
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_get_txwi);
+
+void rt2800mmio_write_tx_desc(struct queue_entry *entry,
+                              struct txentry_desc *txdesc)
+{
+        struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
+        struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
+        __le32 *txd = entry_priv->desc;
+        u32 word;
+        const unsigned int txwi_size = entry->queue->winfo_size;
+
+        /*
+         * The buffers pointed by SD_PTR0/SD_LEN0 and SD_PTR1/SD_LEN1
+         * must contains a TXWI structure + 802.11 header + padding + 802.11
+         * data. We choose to have SD_PTR0/SD_LEN0 only contains TXWI and
+         * SD_PTR1/SD_LEN1 contains 802.11 header + padding + 802.11
+         * data. It means that LAST_SEC0 is always 0.
+         */
+
+        /*
+         * Initialize TX descriptor
+         */
+        word = 0;
+        rt2x00_set_field32(&word, TXD_W0_SD_PTR0, skbdesc->skb_dma);
+        rt2x00_desc_write(txd, 0, word);
+
+        word = 0;
+        rt2x00_set_field32(&word, TXD_W1_SD_LEN1, entry->skb->len);
+        rt2x00_set_field32(&word, TXD_W1_LAST_SEC1,
+                           !test_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags));
+        rt2x00_set_field32(&word, TXD_W1_BURST,
+                           test_bit(ENTRY_TXD_BURST, &txdesc->flags));
+        rt2x00_set_field32(&word, TXD_W1_SD_LEN0, txwi_size);
+        rt2x00_set_field32(&word, TXD_W1_LAST_SEC0, 0);
+        rt2x00_set_field32(&word, TXD_W1_DMA_DONE, 0);
+        rt2x00_desc_write(txd, 1, word);
+
+        word = 0;
+        rt2x00_set_field32(&word, TXD_W2_SD_PTR1,
+                           skbdesc->skb_dma + txwi_size);
+        rt2x00_desc_write(txd, 2, word);
+
+        word = 0;
+        rt2x00_set_field32(&word, TXD_W3_WIV,
+                           !test_bit(ENTRY_TXD_ENCRYPT_IV, &txdesc->flags));
+        rt2x00_set_field32(&word, TXD_W3_QSEL, 2);
+        rt2x00_desc_write(txd, 3, word);
+
+        /*
+         * Register descriptor details in skb frame descriptor.
+         */
+        skbdesc->desc = txd;
+        skbdesc->desc_len = TXD_DESC_SIZE;
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_write_tx_desc);
+
+/*
+ * RX control handlers
+ */
+void rt2800mmio_fill_rxdone(struct queue_entry *entry,
+                            struct rxdone_entry_desc *rxdesc)
+{
+        struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
+        __le32 *rxd = entry_priv->desc;
+        u32 word;
+
+        rt2x00_desc_read(rxd, 3, &word);
+
+        if (rt2x00_get_field32(word, RXD_W3_CRC_ERROR))
+                rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
+
+        /*
+         * Unfortunately we don't know the cipher type used during
+         * decryption. This prevents us from correct providing
+         * correct statistics through debugfs.
+         */
+        rxdesc->cipher_status = rt2x00_get_field32(word, RXD_W3_CIPHER_ERROR);
+
+        if (rt2x00_get_field32(word, RXD_W3_DECRYPTED)) {
+                /*
+                 * Hardware has stripped IV/EIV data from 802.11 frame during
+                 * decryption. Unfortunately the descriptor doesn't contain
+                 * any fields with the EIV/IV data either, so they can't
+                 * be restored by rt2x00lib.
+                 */
+                rxdesc->flags |= RX_FLAG_IV_STRIPPED;
+
+                /*
+                 * The hardware has already checked the Michael Mic and has
+                 * stripped it from the frame. Signal this to mac80211.
+                 */
+                rxdesc->flags |= RX_FLAG_MMIC_STRIPPED;
+
+                if (rxdesc->cipher_status == RX_CRYPTO_SUCCESS)
+                        rxdesc->flags |= RX_FLAG_DECRYPTED;
+                else if (rxdesc->cipher_status == RX_CRYPTO_FAIL_MIC)
+                        rxdesc->flags |= RX_FLAG_MMIC_ERROR;
+        }
+
+        if (rt2x00_get_field32(word, RXD_W3_MY_BSS))
+                rxdesc->dev_flags |= RXDONE_MY_BSS;
+
+        if (rt2x00_get_field32(word, RXD_W3_L2PAD))
+                rxdesc->dev_flags |= RXDONE_L2PAD;
+
+        /*
+         * Process the RXWI structure that is at the start of the buffer.
+         */
+        rt2800_process_rxwi(entry, rxdesc);
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_fill_rxdone);
+
+/*
+ * Interrupt functions.
+ */
+static void rt2800mmio_wakeup(struct rt2x00_dev *rt2x00dev)
+{
+        struct ieee80211_conf conf = { .flags = 0 };
+        struct rt2x00lib_conf libconf = { .conf = &conf };
+
+        rt2800_config(rt2x00dev, &libconf, IEEE80211_CONF_CHANGE_PS);
+}
+
+static bool rt2800mmio_txdone_entry_check(struct queue_entry *entry, u32 status)
+{
+        __le32 *txwi;
+        u32 word;
+        int wcid, tx_wcid;
+
+        wcid = rt2x00_get_field32(status, TX_STA_FIFO_WCID);
+
+        txwi = rt2800_drv_get_txwi(entry);
+        rt2x00_desc_read(txwi, 1, &word);
+        tx_wcid = rt2x00_get_field32(word, TXWI_W1_WIRELESS_CLI_ID);
+
+        return (tx_wcid == wcid);
+}
+
+static bool rt2800mmio_txdone_find_entry(struct queue_entry *entry, void *data)
+{
+        u32 status = *(u32 *)data;
+
+        /*
+         * rt2800pci hardware might reorder frames when exchanging traffic
+         * with multiple BA enabled STAs.
+         *
+         * For example, a tx queue
+         *    [ STA1 | STA2 | STA1 | STA2 ]
+         * can result in tx status reports
+         *    [ STA1 | STA1 | STA2 | STA2 ]
+         * when the hw decides to aggregate the frames for STA1 into one AMPDU.
+         *
+         * To mitigate this effect, associate the tx status to the first frame
+         * in the tx queue with a matching wcid.
+         */
+        if (rt2800mmio_txdone_entry_check(entry, status) &&
+            !test_bit(ENTRY_DATA_STATUS_SET, &entry->flags)) {
+                /*
+                 * Got a matching frame, associate the tx status with
+                 * the frame
+                 */
+                entry->status = status;
+                set_bit(ENTRY_DATA_STATUS_SET, &entry->flags);
+                return true;
+        }
+
+        /* Check the next frame */
+        return false;
+}
+
+static bool rt2800mmio_txdone_match_first(struct queue_entry *entry, void *data)
+{
+        u32 status = *(u32 *)data;
+
+        /*
+         * Find the first frame without tx status and assign this status to it
+         * regardless if it matches or not.
+         */
+        if (!test_bit(ENTRY_DATA_STATUS_SET, &entry->flags)) {
+                /*
+                 * Got a matching frame, associate the tx status with
+                 * the frame
+                 */
+                entry->status = status;
+                set_bit(ENTRY_DATA_STATUS_SET, &entry->flags);
+                return true;
+        }
+
+        /* Check the next frame */
+        return false;
+}
+static bool rt2800mmio_txdone_release_entries(struct queue_entry *entry,
+                                              void *data)
+{
+        if (test_bit(ENTRY_DATA_STATUS_SET, &entry->flags)) {
+                rt2800_txdone_entry(entry, entry->status,
+                                    rt2800mmio_get_txwi(entry));
+                return false;
+        }
+
+        /* No more frames to release */
+        return true;
+}
+
+static bool rt2800mmio_txdone(struct rt2x00_dev *rt2x00dev)
+{
+        struct data_queue *queue;
+        u32 status;
+        u8 qid;
+        int max_tx_done = 16;
+
+        while (kfifo_get(&rt2x00dev->txstatus_fifo, &status)) {
+                qid = rt2x00_get_field32(status, TX_STA_FIFO_PID_QUEUE);
+                if (unlikely(qid >= QID_RX)) {
+                        /*
+                         * Unknown queue, this shouldn't happen. Just drop
+                         * this tx status.
+                         */
+                        rt2x00_warn(rt2x00dev, "Got TX status report with unexpected pid %u, dropping\n",
+                                    qid);
+                        break;
+                }
+
+                queue = rt2x00queue_get_tx_queue(rt2x00dev, qid);
+                if (unlikely(queue == NULL)) {
+                        /*
+                         * The queue is NULL, this shouldn't happen. Stop
+                         * processing here and drop the tx status
+                         */
+                        rt2x00_warn(rt2x00dev, "Got TX status for an unavailable queue %u, dropping\n",
+                                    qid);
+                        break;
+                }
+
+                if (unlikely(rt2x00queue_empty(queue))) {
+                        /*
+                         * The queue is empty. Stop processing here
+                         * and drop the tx status.
+                         */
+                        rt2x00_warn(rt2x00dev, "Got TX status for an empty queue %u, dropping\n",
+                                    qid);
+                        break;
+                }
+
+                /*
+                 * Let's associate this tx status with the first
+                 * matching frame.
+                 */
+                if (!rt2x00queue_for_each_entry(queue, Q_INDEX_DONE,
+                                                Q_INDEX, &status,
+                                                rt2800mmio_txdone_find_entry)) {
+                        /*
+                         * We cannot match the tx status to any frame, so just
+                         * use the first one.
+                         */
+                        if (!rt2x00queue_for_each_entry(queue, Q_INDEX_DONE,
+                                                        Q_INDEX, &status,
+                                                        rt2800mmio_txdone_match_first)) {
+                                rt2x00_warn(rt2x00dev, "No frame found for TX status on queue %u, dropping\n",
+                                            qid);
+                                break;
+                        }
+                }
+
+                /*
+                 * Release all frames with a valid tx status.
+                 */
+                rt2x00queue_for_each_entry(queue, Q_INDEX_DONE,
+                                           Q_INDEX, NULL,
+                                           rt2800mmio_txdone_release_entries);
+
+                if (--max_tx_done == 0)
+                        break;
+        }
+
+        return !max_tx_done;
+}
+
+static inline void rt2800mmio_enable_interrupt(struct rt2x00_dev *rt2x00dev,
+                                               struct rt2x00_field32 irq_field)
+{
+        u32 reg;
+
+        /*
+         * Enable a single interrupt. The interrupt mask register
+         * access needs locking.
+         */
+        spin_lock_irq(&rt2x00dev->irqmask_lock);
+        rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+        rt2x00_set_field32(&reg, irq_field, 1);
+        rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
+        spin_unlock_irq(&rt2x00dev->irqmask_lock);
+}
+
+void rt2800mmio_txstatus_tasklet(unsigned long data)
+{
+        struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+        if (rt2800mmio_txdone(rt2x00dev))
+                tasklet_schedule(&rt2x00dev->txstatus_tasklet);
+
+        /*
+         * No need to enable the tx status interrupt here as we always
+         * leave it enabled to minimize the possibility of a tx status
+         * register overflow. See comment in interrupt handler.
+         */
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_txstatus_tasklet);
+
+void rt2800mmio_pretbtt_tasklet(unsigned long data)
+{
+        struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+        rt2x00lib_pretbtt(rt2x00dev);
+        if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
+                rt2800mmio_enable_interrupt(rt2x00dev, INT_MASK_CSR_PRE_TBTT);
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_pretbtt_tasklet);
+
+void rt2800mmio_tbtt_tasklet(unsigned long data)
+{
+        struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+        struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
+        u32 reg;
+
+        rt2x00lib_beacondone(rt2x00dev);
+
+        if (rt2x00dev->intf_ap_count) {
+                /*
+                 * The rt2800pci hardware tbtt timer is off by 1us per tbtt
+                 * causing beacon skew and as a result causing problems with
+                 * some powersaving clients over time. Shorten the beacon
+                 * interval every 64 beacons by 64us to mitigate this effect.
+                 */
+                if (drv_data->tbtt_tick == (BCN_TBTT_OFFSET - 2)) {
+                        rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+                        rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_INTERVAL,
+                                           (rt2x00dev->beacon_int * 16) - 1);
+                        rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+                } else if (drv_data->tbtt_tick == (BCN_TBTT_OFFSET - 1)) {
+                        rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+                        rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_INTERVAL,
+                                           (rt2x00dev->beacon_int * 16));
+                        rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+                }
+                drv_data->tbtt_tick++;
+                drv_data->tbtt_tick %= BCN_TBTT_OFFSET;
+        }
+
+        if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
+                rt2800mmio_enable_interrupt(rt2x00dev, INT_MASK_CSR_TBTT);
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_tbtt_tasklet);
+
+void rt2800mmio_rxdone_tasklet(unsigned long data)
+{
+        struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+        if (rt2x00mmio_rxdone(rt2x00dev))
+                tasklet_schedule(&rt2x00dev->rxdone_tasklet);
+        else if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
+                rt2800mmio_enable_interrupt(rt2x00dev, INT_MASK_CSR_RX_DONE);
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_rxdone_tasklet);
+
+void rt2800mmio_autowake_tasklet(unsigned long data)
+{
+        struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+        rt2800mmio_wakeup(rt2x00dev);
+        if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
+                rt2800mmio_enable_interrupt(rt2x00dev,
+                                            INT_MASK_CSR_AUTO_WAKEUP);
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_autowake_tasklet);
+
+static void rt2800mmio_txstatus_interrupt(struct rt2x00_dev *rt2x00dev)
+{
+        u32 status;
+        int i;
+
+        /*
+         * The TX_FIFO_STATUS interrupt needs special care. We should
+         * read TX_STA_FIFO but we should do it immediately as otherwise
+         * the register can overflow and we would lose status reports.
+         *
+         * Hence, read the TX_STA_FIFO register and copy all tx status
+         * reports into a kernel FIFO which is handled in the txstatus
+         * tasklet. We use a tasklet to process the tx status reports
+         * because we can schedule the tasklet multiple times (when the
+         * interrupt fires again during tx status processing).
+         *
+         * Furthermore we don't disable the TX_FIFO_STATUS
+         * interrupt here but leave it enabled so that the TX_STA_FIFO
+         * can also be read while the tx status tasklet gets executed.
+         *
+         * Since we have only one producer and one consumer we don't
+         * need to lock the kfifo.
+         */
+        for (i = 0; i < rt2x00dev->tx->limit; i++) {
+                rt2x00mmio_register_read(rt2x00dev, TX_STA_FIFO, &status);
+
+                if (!rt2x00_get_field32(status, TX_STA_FIFO_VALID))
+                        break;
+
+                if (!kfifo_put(&rt2x00dev->txstatus_fifo, &status)) {
+                        rt2x00_warn(rt2x00dev, "TX status FIFO overrun, drop tx status report\n");
+                        break;
+                }
+        }
+
+        /* Schedule the tasklet for processing the tx status. */
+        tasklet_schedule(&rt2x00dev->txstatus_tasklet);
+}
+
+irqreturn_t rt2800mmio_interrupt(int irq, void *dev_instance)
+{
+        struct rt2x00_dev *rt2x00dev = dev_instance;
+        u32 reg, mask;
+
+        /* Read status and ACK all interrupts */
+        rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
+        rt2x00mmio_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
+
+        if (!reg)
+                return IRQ_NONE;
+
+        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
+                return IRQ_HANDLED;
+
+        /*
+         * Since INT_MASK_CSR and INT_SOURCE_CSR use the same bits
+         * for interrupts and interrupt masks we can just use the value of
+         * INT_SOURCE_CSR to create the interrupt mask.
+         */
+        mask = ~reg;
+
+        if (rt2x00_get_field32(reg, INT_SOURCE_CSR_TX_FIFO_STATUS)) {
+                rt2800mmio_txstatus_interrupt(rt2x00dev);
+                /*
+                 * Never disable the TX_FIFO_STATUS interrupt.
+                 */
+                rt2x00_set_field32(&mask, INT_MASK_CSR_TX_FIFO_STATUS, 1);
+        }
+
+        if (rt2x00_get_field32(reg, INT_SOURCE_CSR_PRE_TBTT))
+                tasklet_hi_schedule(&rt2x00dev->pretbtt_tasklet);
+
+        if (rt2x00_get_field32(reg, INT_SOURCE_CSR_TBTT))
+                tasklet_hi_schedule(&rt2x00dev->tbtt_tasklet);
+
+        if (rt2x00_get_field32(reg, INT_SOURCE_CSR_RX_DONE))
+                tasklet_schedule(&rt2x00dev->rxdone_tasklet);
+
+        if (rt2x00_get_field32(reg, INT_SOURCE_CSR_AUTO_WAKEUP))
+                tasklet_schedule(&rt2x00dev->autowake_tasklet);
+
+        /*
+         * Disable all interrupts for which a tasklet was scheduled right now,
+         * the tasklet will reenable the appropriate interrupts.
+         */
+        spin_lock(&rt2x00dev->irqmask_lock);
+        rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+        reg &= mask;
+        rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
+        spin_unlock(&rt2x00dev->irqmask_lock);
+
+        return IRQ_HANDLED;
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_interrupt);
+
+void rt2800mmio_toggle_irq(struct rt2x00_dev *rt2x00dev,
+                           enum dev_state state)
+{
+        u32 reg;
+        unsigned long flags;
+
+        /*
+         * When interrupts are being enabled, the interrupt registers
+         * should clear the register to assure a clean state.
+         */
+        if (state == STATE_RADIO_IRQ_ON) {
+                rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
+                rt2x00mmio_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
+        }
+
+        spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
+        reg = 0;
+        if (state == STATE_RADIO_IRQ_ON) {
+                rt2x00_set_field32(&reg, INT_MASK_CSR_RX_DONE, 1);
+                rt2x00_set_field32(&reg, INT_MASK_CSR_TBTT, 1);
+                rt2x00_set_field32(&reg, INT_MASK_CSR_PRE_TBTT, 1);
+                rt2x00_set_field32(&reg, INT_MASK_CSR_TX_FIFO_STATUS, 1);
+                rt2x00_set_field32(&reg, INT_MASK_CSR_AUTO_WAKEUP, 1);
+        }
+        rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
+        spin_unlock_irqrestore(&rt2x00dev->irqmask_lock, flags);
+
+        if (state == STATE_RADIO_IRQ_OFF) {
+                /*
+                 * Wait for possibly running tasklets to finish.
+                 */
+                tasklet_kill(&rt2x00dev->txstatus_tasklet);
+                tasklet_kill(&rt2x00dev->rxdone_tasklet);
+                tasklet_kill(&rt2x00dev->autowake_tasklet);
+                tasklet_kill(&rt2x00dev->tbtt_tasklet);
+                tasklet_kill(&rt2x00dev->pretbtt_tasklet);
+        }
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_toggle_irq);
+
+/*
+ * Queue handlers.
+ */
+void rt2800mmio_start_queue(struct data_queue *queue)
+{
+        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
+        u32 reg;
+
+        switch (queue->qid) {
+        case QID_RX:
+                rt2x00mmio_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
+                rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 1);
+                rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
+                break;
+        case QID_BEACON:
+                rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+                rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 1);
+                rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 1);
+                rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 1);
+                rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+
+                rt2x00mmio_register_read(rt2x00dev, INT_TIMER_EN, &reg);
+                rt2x00_set_field32(&reg, INT_TIMER_EN_PRE_TBTT_TIMER, 1);
+                rt2x00mmio_register_write(rt2x00dev, INT_TIMER_EN, reg);
+                break;
+        default:
+                break;
+        }
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_start_queue);
+
+void rt2800mmio_kick_queue(struct data_queue *queue)
+{
+        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
+        struct queue_entry *entry;
+
+        switch (queue->qid) {
+        case QID_AC_VO:
+        case QID_AC_VI:
+        case QID_AC_BE:
+        case QID_AC_BK:
+                entry = rt2x00queue_get_entry(queue, Q_INDEX);
+                rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX(queue->qid),
+                                          entry->entry_idx);
+                break;
+        case QID_MGMT:
+                entry = rt2x00queue_get_entry(queue, Q_INDEX);
+                rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX(5),
+                                          entry->entry_idx);
+                break;
+        default:
+                break;
+        }
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_kick_queue);
+
+void rt2800mmio_stop_queue(struct data_queue *queue)
+{
+        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
+        u32 reg;
+
+        switch (queue->qid) {
+        case QID_RX:
+                rt2x00mmio_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
+                rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 0);
+                rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
+                break;
+        case QID_BEACON:
+                rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+                rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 0);
+                rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 0);
+                rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 0);
+                rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+
+                rt2x00mmio_register_read(rt2x00dev, INT_TIMER_EN, &reg);
+                rt2x00_set_field32(&reg, INT_TIMER_EN_PRE_TBTT_TIMER, 0);
+                rt2x00mmio_register_write(rt2x00dev, INT_TIMER_EN, reg);
+
+                /*
+                 * Wait for current invocation to finish. The tasklet
+                 * won't be scheduled anymore afterwards since we disabled
+                 * the TBTT and PRE TBTT timer.
+                 */
+                tasklet_kill(&rt2x00dev->tbtt_tasklet);
+                tasklet_kill(&rt2x00dev->pretbtt_tasklet);
+
+                break;
+        default:
+                break;
+        }
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_stop_queue);
+
+void rt2800mmio_queue_init(struct data_queue *queue)
+{
+        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
+        unsigned short txwi_size, rxwi_size;
+
+        rt2800_get_txwi_rxwi_size(rt2x00dev, &txwi_size, &rxwi_size);
+
+        switch (queue->qid) {
+        case QID_RX:
+                queue->limit = 128;
+                queue->data_size = AGGREGATION_SIZE;
+                queue->desc_size = RXD_DESC_SIZE;
+                queue->winfo_size = rxwi_size;
+                queue->priv_size = sizeof(struct queue_entry_priv_mmio);
+                break;
+
+        case QID_AC_VO:
+        case QID_AC_VI:
+        case QID_AC_BE:
+        case QID_AC_BK:
+                queue->limit = 64;
+                queue->data_size = AGGREGATION_SIZE;
+                queue->desc_size = TXD_DESC_SIZE;
+                queue->winfo_size = txwi_size;
+                queue->priv_size = sizeof(struct queue_entry_priv_mmio);
+                break;
+
+        case QID_BEACON:
+                queue->limit = 8;
+                queue->data_size = 0; /* No DMA required for beacons */
+                queue->desc_size = TXD_DESC_SIZE;
+                queue->winfo_size = txwi_size;
+                queue->priv_size = sizeof(struct queue_entry_priv_mmio);
+                break;
+
+        case QID_ATIM:
+                /* fallthrough */
+        default:
+                BUG();
+                break;
+        }
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_queue_init);
+
+/*
+ * Initialization functions.
+ */
+bool rt2800mmio_get_entry_state(struct queue_entry *entry)
+{
+        struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
+        u32 word;
+
+        if (entry->queue->qid == QID_RX) {
+                rt2x00_desc_read(entry_priv->desc, 1, &word);
+
+                return (!rt2x00_get_field32(word, RXD_W1_DMA_DONE));
+        } else {
+                rt2x00_desc_read(entry_priv->desc, 1, &word);
+
+                return (!rt2x00_get_field32(word, TXD_W1_DMA_DONE));
+        }
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_get_entry_state);
+
+void rt2800mmio_clear_entry(struct queue_entry *entry)
+{
+        struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
+        struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
+        struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+        u32 word;
+
+        if (entry->queue->qid == QID_RX) {
+                rt2x00_desc_read(entry_priv->desc, 0, &word);
+                rt2x00_set_field32(&word, RXD_W0_SDP0, skbdesc->skb_dma);
+                rt2x00_desc_write(entry_priv->desc, 0, word);
+
+                rt2x00_desc_read(entry_priv->desc, 1, &word);
+                rt2x00_set_field32(&word, RXD_W1_DMA_DONE, 0);
+                rt2x00_desc_write(entry_priv->desc, 1, word);
+
+                /*
+                 * Set RX IDX in register to inform hardware that we have
+                 * handled this entry and it is available for reuse again.
+                 */
+                rt2x00mmio_register_write(rt2x00dev, RX_CRX_IDX,
+                                          entry->entry_idx);
+        } else {
+                rt2x00_desc_read(entry_priv->desc, 1, &word);
+                rt2x00_set_field32(&word, TXD_W1_DMA_DONE, 1);
+                rt2x00_desc_write(entry_priv->desc, 1, word);
+        }
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_clear_entry);
+
+int rt2800mmio_init_queues(struct rt2x00_dev *rt2x00dev)
+{
+        struct queue_entry_priv_mmio *entry_priv;
+
+        /*
+         * Initialize registers.
+         */
+        entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
+        rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR0,
+                                  entry_priv->desc_dma);
+        rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT0,
+                                  rt2x00dev->tx[0].limit);
+        rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX0, 0);
+        rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX0, 0);
+
+        entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
+        rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR1,
+                                  entry_priv->desc_dma);
+        rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT1,
+                                  rt2x00dev->tx[1].limit);
+        rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX1, 0);
+        rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX1, 0);
+
+        entry_priv = rt2x00dev->tx[2].entries[0].priv_data;
+        rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR2,
+                                  entry_priv->desc_dma);
+        rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT2,
+                                  rt2x00dev->tx[2].limit);
+        rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX2, 0);
+        rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX2, 0);
+
+        entry_priv = rt2x00dev->tx[3].entries[0].priv_data;
+        rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR3,
+                                  entry_priv->desc_dma);
+        rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT3,
+                                  rt2x00dev->tx[3].limit);
+        rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX3, 0);
+        rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX3, 0);
+
+        rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR4, 0);
+        rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT4, 0);
+        rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX4, 0);
+        rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX4, 0);
+
+        rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR5, 0);
+        rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT5, 0);
+        rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX5, 0);
+        rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX5, 0);
+
+        entry_priv = rt2x00dev->rx->entries[0].priv_data;
+        rt2x00mmio_register_write(rt2x00dev, RX_BASE_PTR,
+                                  entry_priv->desc_dma);
+        rt2x00mmio_register_write(rt2x00dev, RX_MAX_CNT,
+                                  rt2x00dev->rx[0].limit);
+        rt2x00mmio_register_write(rt2x00dev, RX_CRX_IDX,
+                                  rt2x00dev->rx[0].limit - 1);
+        rt2x00mmio_register_write(rt2x00dev, RX_DRX_IDX, 0);
+
+        rt2800_disable_wpdma(rt2x00dev);
+
+        rt2x00mmio_register_write(rt2x00dev, DELAY_INT_CFG, 0);
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_init_queues);
+
+int rt2800mmio_init_registers(struct rt2x00_dev *rt2x00dev)
+{
+        u32 reg;
+
+        /*
+         * Reset DMA indexes
+         */
+        rt2x00mmio_register_read(rt2x00dev, WPDMA_RST_IDX, &reg);
+        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX0, 1);
+        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX1, 1);
+        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX2, 1);
+        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX3, 1);
+        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX4, 1);
+        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX5, 1);
+        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DRX_IDX0, 1);
+        rt2x00mmio_register_write(rt2x00dev, WPDMA_RST_IDX, reg);
+
+        rt2x00mmio_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000e1f);
+        rt2x00mmio_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000e00);
+
+        if (rt2x00_is_pcie(rt2x00dev) &&
+            (rt2x00_rt(rt2x00dev, RT3090) ||
+             rt2x00_rt(rt2x00dev, RT3390) ||
+             rt2x00_rt(rt2x00dev, RT3572) ||
+             rt2x00_rt(rt2x00dev, RT3593) ||
+             rt2x00_rt(rt2x00dev, RT5390) ||
+             rt2x00_rt(rt2x00dev, RT5392) ||
+             rt2x00_rt(rt2x00dev, RT5592))) {
+                rt2x00mmio_register_read(rt2x00dev, AUX_CTRL, &reg);
+                rt2x00_set_field32(&reg, AUX_CTRL_FORCE_PCIE_CLK, 1);
+                rt2x00_set_field32(&reg, AUX_CTRL_WAKE_PCIE_EN, 1);
+                rt2x00mmio_register_write(rt2x00dev, AUX_CTRL, reg);
+        }
+
+        rt2x00mmio_register_write(rt2x00dev, PWR_PIN_CFG, 0x00000003);
+
+        reg = 0;
+        rt2x00_set_field32(&reg, MAC_SYS_CTRL_RESET_CSR, 1);
+        rt2x00_set_field32(&reg, MAC_SYS_CTRL_RESET_BBP, 1);
+        rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
+
+        rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, 0x00000000);
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_init_registers);
+
+/*
+ * Device state switch handlers.
+ */
+int rt2800mmio_enable_radio(struct rt2x00_dev *rt2x00dev)
+{
+        /* Wait for DMA, ignore error until we initialize queues. */
+        rt2800_wait_wpdma_ready(rt2x00dev);
+
+        if (unlikely(rt2800mmio_init_queues(rt2x00dev)))
+                return -EIO;
+
+        return rt2800_enable_radio(rt2x00dev);
+}
+EXPORT_SYMBOL_GPL(rt2800mmio_enable_radio);
+
+MODULE_AUTHOR(DRV_PROJECT);
+MODULE_VERSION(DRV_VERSION);
+MODULE_DESCRIPTION("rt2800 MMIO library");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wireless/rt2x00/rt2800mmio.h b/drivers/net/wireless/rt2x00/rt2800mmio.h
new file mode 100644
index 000000000000..6a10de3eee3e
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2800mmio.h
@@ -0,0 +1,165 @@
+/*      Copyright (C) 2009 - 2010 Ivo van Doorn <IvDoorn@gmail.com>
+ *      Copyright (C) 2009 Alban Browaeys <prahal@yahoo.com>
+ *      Copyright (C) 2009 Felix Fietkau <nbd@openwrt.org>
+ *      Copyright (C) 2009 Luis Correia <luis.f.correia@gmail.com>
+ *      Copyright (C) 2009 Mattias Nissler <mattias.nissler@gmx.de>
+ *      Copyright (C) 2009 Mark Asselstine <asselsm@gmail.com>
+ *      Copyright (C) 2009 Xose Vazquez Perez <xose.vazquez@gmail.com>
+ *      Copyright (C) 2009 Bart Zolnierkiewicz <bzolnier@gmail.com>
+ *      <http://rt2x00.serialmonkey.com>
+ *
+ *      This program is free software; you can redistribute it and/or modify
+ *      it under the terms of the GNU General Public License as published by
+ *      the Free Software Foundation; either version 2 of the License, or
+ *      (at your option) any later version.
+ *
+ *      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.
+ *
+ *      You should have received a copy of the GNU General Public License
+ *      along with this program; if not, write to the
+ *      Free Software Foundation, Inc.,
+ *      59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*      Module: rt2800mmio
+ *      Abstract: forward declarations for the rt2800mmio module.
+ */
+
+#ifndef RT2800MMIO_H
+#define RT2800MMIO_H
+
+/*
+ * Queue register offset macros
+ */
+#define TX_QUEUE_REG_OFFSET     0x10
+#define TX_BASE_PTR(__x)        (TX_BASE_PTR0 + ((__x) * TX_QUEUE_REG_OFFSET))
+#define TX_MAX_CNT(__x)         (TX_MAX_CNT0 + ((__x) * TX_QUEUE_REG_OFFSET))
+#define TX_CTX_IDX(__x)         (TX_CTX_IDX0 + ((__x) * TX_QUEUE_REG_OFFSET))
+#define TX_DTX_IDX(__x)         (TX_DTX_IDX0 + ((__x) * TX_QUEUE_REG_OFFSET))
+
+/*
+ * DMA descriptor defines.
+ */
+#define TXD_DESC_SIZE                   (4 * sizeof(__le32))
+#define RXD_DESC_SIZE                   (4 * sizeof(__le32))
+
+/*
+ * TX descriptor format for TX, PRIO and Beacon Ring.
+ */
+
+/*
+ * Word0
+ */
+#define TXD_W0_SD_PTR0                  FIELD32(0xffffffff)
+
+/*
+ * Word1
+ */
+#define TXD_W1_SD_LEN1                  FIELD32(0x00003fff)
+#define TXD_W1_LAST_SEC1                FIELD32(0x00004000)
+#define TXD_W1_BURST                    FIELD32(0x00008000)
+#define TXD_W1_SD_LEN0                  FIELD32(0x3fff0000)
+#define TXD_W1_LAST_SEC0                FIELD32(0x40000000)
+#define TXD_W1_DMA_DONE                 FIELD32(0x80000000)
+
+/*
+ * Word2
+ */
+#define TXD_W2_SD_PTR1                  FIELD32(0xffffffff)
+
+/*
+ * Word3
+ * WIV: Wireless Info Valid. 1: Driver filled WI, 0: DMA needs to copy WI
+ * QSEL: Select on-chip FIFO ID for 2nd-stage output scheduler.
+ *       0:MGMT, 1:HCCA 2:EDCA
+ */
+#define TXD_W3_WIV                      FIELD32(0x01000000)
+#define TXD_W3_QSEL                     FIELD32(0x06000000)
+#define TXD_W3_TCO                      FIELD32(0x20000000)
+#define TXD_W3_UCO                      FIELD32(0x40000000)
+#define TXD_W3_ICO                      FIELD32(0x80000000)
+
+/*
+ * RX descriptor format for RX Ring.
+ */
+
+/*
+ * Word0
+ */
+#define RXD_W0_SDP0                     FIELD32(0xffffffff)
+
+/*
+ * Word1
+ */
+#define RXD_W1_SDL1                     FIELD32(0x00003fff)
+#define RXD_W1_SDL0                     FIELD32(0x3fff0000)
+#define RXD_W1_LS0                      FIELD32(0x40000000)
+#define RXD_W1_DMA_DONE                 FIELD32(0x80000000)
+
+/*
+ * Word2
+ */
+#define RXD_W2_SDP1                     FIELD32(0xffffffff)
+
+/*
+ * Word3
+ * AMSDU: RX with 802.3 header, not 802.11 header.
+ * DECRYPTED: This frame is being decrypted.
+ */
+#define RXD_W3_BA                       FIELD32(0x00000001)
+#define RXD_W3_DATA                     FIELD32(0x00000002)
+#define RXD_W3_NULLDATA                 FIELD32(0x00000004)
+#define RXD_W3_FRAG                     FIELD32(0x00000008)
+#define RXD_W3_UNICAST_TO_ME            FIELD32(0x00000010)
+#define RXD_W3_MULTICAST                FIELD32(0x00000020)
+#define RXD_W3_BROADCAST                FIELD32(0x00000040)
+#define RXD_W3_MY_BSS                   FIELD32(0x00000080)
+#define RXD_W3_CRC_ERROR                FIELD32(0x00000100)
+#define RXD_W3_CIPHER_ERROR             FIELD32(0x00000600)
+#define RXD_W3_AMSDU                    FIELD32(0x00000800)
+#define RXD_W3_HTC                      FIELD32(0x00001000)
+#define RXD_W3_RSSI                     FIELD32(0x00002000)
+#define RXD_W3_L2PAD                    FIELD32(0x00004000)
+#define RXD_W3_AMPDU                    FIELD32(0x00008000)
+#define RXD_W3_DECRYPTED                FIELD32(0x00010000)
+#define RXD_W3_PLCP_SIGNAL              FIELD32(0x00020000)
+#define RXD_W3_PLCP_RSSI                FIELD32(0x00040000)
+
+/* TX descriptor initialization */
+__le32 *rt2800mmio_get_txwi(struct queue_entry *entry);
+void rt2800mmio_write_tx_desc(struct queue_entry *entry,
+                              struct txentry_desc *txdesc);
+
+/* RX control handlers */
+void rt2800mmio_fill_rxdone(struct queue_entry *entry,
+                            struct rxdone_entry_desc *rxdesc);
+
+/* Interrupt functions */
+void rt2800mmio_txstatus_tasklet(unsigned long data);
+void rt2800mmio_pretbtt_tasklet(unsigned long data);
+void rt2800mmio_tbtt_tasklet(unsigned long data);
+void rt2800mmio_rxdone_tasklet(unsigned long data);
+void rt2800mmio_autowake_tasklet(unsigned long data);
+irqreturn_t rt2800mmio_interrupt(int irq, void *dev_instance);
+void rt2800mmio_toggle_irq(struct rt2x00_dev *rt2x00dev,
+                           enum dev_state state);
+
+/* Queue handlers */
+void rt2800mmio_start_queue(struct data_queue *queue);
+void rt2800mmio_kick_queue(struct data_queue *queue);
+void rt2800mmio_stop_queue(struct data_queue *queue);
+void rt2800mmio_queue_init(struct data_queue *queue);
+
+/* Initialization functions */
+bool rt2800mmio_get_entry_state(struct queue_entry *entry);
+void rt2800mmio_clear_entry(struct queue_entry *entry);
+int rt2800mmio_init_queues(struct rt2x00_dev *rt2x00dev);
+int rt2800mmio_init_registers(struct rt2x00_dev *rt2x00dev);
+
+/* Device state switch handlers. */
+int rt2800mmio_enable_radio(struct rt2x00_dev *rt2x00dev);
+
+#endif /* RT2800MMIO_H */
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index f8f2abbfbb65..b504455b4fec 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -37,14 +37,13 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/pci.h>
-#include <linux/platform_device.h>
 #include <linux/eeprom_93cx6.h>
 
 #include "rt2x00.h"
 #include "rt2x00mmio.h"
 #include "rt2x00pci.h"
-#include "rt2x00soc.h"
 #include "rt2800lib.h"
+#include "rt2800mmio.h"
 #include "rt2800.h"
 #include "rt2800pci.h"
 
@@ -90,27 +89,6 @@ static void rt2800pci_mcu_status(struct rt2x00_dev *rt2x00dev, const u8 token)
         rt2x00mmio_register_write(rt2x00dev, H2M_MAILBOX_CID, ~0);
 }
 
-#if defined(CONFIG_SOC_RT288X) || defined(CONFIG_SOC_RT305X)
-static int rt2800pci_read_eeprom_soc(struct rt2x00_dev *rt2x00dev)
-{
-        void __iomem *base_addr = ioremap(0x1F040000, EEPROM_SIZE);
-
-        if (!base_addr)
-                return -ENOMEM;
-
-        memcpy_fromio(rt2x00dev->eeprom, base_addr, EEPROM_SIZE);
-
-        iounmap(base_addr);
-        return 0;
-}
-#else
-static inline int rt2800pci_read_eeprom_soc(struct rt2x00_dev *rt2x00dev)
-{
-        return -ENOMEM;
-}
-#endif /* CONFIG_SOC_RT288X || CONFIG_SOC_RT305X */
-
-#ifdef CONFIG_PCI
 static void rt2800pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
 {
         struct rt2x00_dev *rt2x00dev = eeprom->data;
@@ -183,112 +161,6 @@ static inline int rt2800pci_read_eeprom_efuse(struct rt2x00_dev *rt2x00dev)
 {
         return rt2800_read_eeprom_efuse(rt2x00dev);
 }
-#else
-static inline int rt2800pci_read_eeprom_pci(struct rt2x00_dev *rt2x00dev)
-{
-        return -EOPNOTSUPP;
-}
-
-static inline int rt2800pci_efuse_detect(struct rt2x00_dev *rt2x00dev)
-{
-        return 0;
-}
-
-static inline int rt2800pci_read_eeprom_efuse(struct rt2x00_dev *rt2x00dev)
-{
-        return -EOPNOTSUPP;
-}
-#endif /* CONFIG_PCI */
-
-/*
- * Queue handlers.
- */
-static void rt2800pci_start_queue(struct data_queue *queue)
-{
-        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
-        u32 reg;
-
-        switch (queue->qid) {
-        case QID_RX:
-                rt2x00mmio_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
-                rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 1);
-                rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
-                break;
-        case QID_BEACON:
-                rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
-                rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 1);
-                rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 1);
-                rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 1);
-                rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
-
-                rt2x00mmio_register_read(rt2x00dev, INT_TIMER_EN, &reg);
-                rt2x00_set_field32(&reg, INT_TIMER_EN_PRE_TBTT_TIMER, 1);
-                rt2x00mmio_register_write(rt2x00dev, INT_TIMER_EN, reg);
-                break;
-        default:
-                break;
-        }
-}
-
-static void rt2800pci_kick_queue(struct data_queue *queue)
-{
-        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
-        struct queue_entry *entry;
-
-        switch (queue->qid) {
-        case QID_AC_VO:
-        case QID_AC_VI:
-        case QID_AC_BE:
-        case QID_AC_BK:
-                entry = rt2x00queue_get_entry(queue, Q_INDEX);
-                rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX(queue->qid),
-                                          entry->entry_idx);
-                break;
-        case QID_MGMT:
-                entry = rt2x00queue_get_entry(queue, Q_INDEX);
-                rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX(5),
-                                          entry->entry_idx);
-                break;
-        default:
-                break;
-        }
-}
-
-static void rt2800pci_stop_queue(struct data_queue *queue)
-{
-        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
-        u32 reg;
-
-        switch (queue->qid) {
-        case QID_RX:
-                rt2x00mmio_register_read(rt2x00dev, MAC_SYS_CTRL, &reg);
-                rt2x00_set_field32(&reg, MAC_SYS_CTRL_ENABLE_RX, 0);
-                rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
-                break;
-        case QID_BEACON:
-                rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
-                rt2x00_set_field32(&reg, BCN_TIME_CFG_TSF_TICKING, 0);
-                rt2x00_set_field32(&reg, BCN_TIME_CFG_TBTT_ENABLE, 0);
-                rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 0);
-                rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
-
-                rt2x00mmio_register_read(rt2x00dev, INT_TIMER_EN, &reg);
-                rt2x00_set_field32(&reg, INT_TIMER_EN_PRE_TBTT_TIMER, 0);
-                rt2x00mmio_register_write(rt2x00dev, INT_TIMER_EN, reg);
-
-                /*
-                 * Wait for current invocation to finish. The tasklet
-                 * won't be scheduled anymore afterwards since we disabled
-                 * the TBTT and PRE TBTT timer.
-                 */
-                tasklet_kill(&rt2x00dev->tbtt_tasklet);
-                tasklet_kill(&rt2x00dev->pretbtt_tasklet);
-
-                break;
-        default:
-                break;
-        }
-}
 
 /*
  * Firmware functions
@@ -332,217 +204,13 @@ static int rt2800pci_write_firmware(struct rt2x00_dev *rt2x00dev,
 }
 
 /*
- * Initialization functions.
- */
-static bool rt2800pci_get_entry_state(struct queue_entry *entry)
-{
-        struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
-        u32 word;
-
-        if (entry->queue->qid == QID_RX) {
-                rt2x00_desc_read(entry_priv->desc, 1, &word);
-
-                return (!rt2x00_get_field32(word, RXD_W1_DMA_DONE));
-        } else {
-                rt2x00_desc_read(entry_priv->desc, 1, &word);
-
-                return (!rt2x00_get_field32(word, TXD_W1_DMA_DONE));
-        }
-}
-
-static void rt2800pci_clear_entry(struct queue_entry *entry)
-{
-        struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
-        struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
-        struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
-        u32 word;
-
-        if (entry->queue->qid == QID_RX) {
-                rt2x00_desc_read(entry_priv->desc, 0, &word);
-                rt2x00_set_field32(&word, RXD_W0_SDP0, skbdesc->skb_dma);
-                rt2x00_desc_write(entry_priv->desc, 0, word);
-
-                rt2x00_desc_read(entry_priv->desc, 1, &word);
-                rt2x00_set_field32(&word, RXD_W1_DMA_DONE, 0);
-                rt2x00_desc_write(entry_priv->desc, 1, word);
-
-                /*
-                 * Set RX IDX in register to inform hardware that we have
-                 * handled this entry and it is available for reuse again.
-                 */
-                rt2x00mmio_register_write(rt2x00dev, RX_CRX_IDX,
-                                          entry->entry_idx);
-        } else {
-                rt2x00_desc_read(entry_priv->desc, 1, &word);
-                rt2x00_set_field32(&word, TXD_W1_DMA_DONE, 1);
-                rt2x00_desc_write(entry_priv->desc, 1, word);
-        }
-}
-
-static int rt2800pci_init_queues(struct rt2x00_dev *rt2x00dev)
-{
-        struct queue_entry_priv_mmio *entry_priv;
-
-        /*
-         * Initialize registers.
-         */
-        entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
-        rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR0,
-                                  entry_priv->desc_dma);
-        rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT0,
-                                  rt2x00dev->tx[0].limit);
-        rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX0, 0);
-        rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX0, 0);
-
-        entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
-        rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR1,
-                                  entry_priv->desc_dma);
-        rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT1,
-                                  rt2x00dev->tx[1].limit);
-        rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX1, 0);
-        rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX1, 0);
-
-        entry_priv = rt2x00dev->tx[2].entries[0].priv_data;
-        rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR2,
-                                  entry_priv->desc_dma);
-        rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT2,
-                                  rt2x00dev->tx[2].limit);
-        rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX2, 0);
-        rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX2, 0);
-
-        entry_priv = rt2x00dev->tx[3].entries[0].priv_data;
-        rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR3,
-                                  entry_priv->desc_dma);
-        rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT3,
-                                  rt2x00dev->tx[3].limit);
-        rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX3, 0);
-        rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX3, 0);
-
-        rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR4, 0);
-        rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT4, 0);
-        rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX4, 0);
-        rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX4, 0);
-
-        rt2x00mmio_register_write(rt2x00dev, TX_BASE_PTR5, 0);
-        rt2x00mmio_register_write(rt2x00dev, TX_MAX_CNT5, 0);
-        rt2x00mmio_register_write(rt2x00dev, TX_CTX_IDX5, 0);
-        rt2x00mmio_register_write(rt2x00dev, TX_DTX_IDX5, 0);
-
-        entry_priv = rt2x00dev->rx->entries[0].priv_data;
-        rt2x00mmio_register_write(rt2x00dev, RX_BASE_PTR,
-                                  entry_priv->desc_dma);
-        rt2x00mmio_register_write(rt2x00dev, RX_MAX_CNT,
-                                  rt2x00dev->rx[0].limit);
-        rt2x00mmio_register_write(rt2x00dev, RX_CRX_IDX,
-                                  rt2x00dev->rx[0].limit - 1);
-        rt2x00mmio_register_write(rt2x00dev, RX_DRX_IDX, 0);
-
-        rt2800_disable_wpdma(rt2x00dev);
-
-        rt2x00mmio_register_write(rt2x00dev, DELAY_INT_CFG, 0);
-
-        return 0;
-}
-
-/*
  * Device state switch handlers.
  */
-static void rt2800pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
-                                 enum dev_state state)
-{
-        u32 reg;
-        unsigned long flags;
-
-        /*
-         * When interrupts are being enabled, the interrupt registers
-         * should clear the register to assure a clean state.
-         */
-        if (state == STATE_RADIO_IRQ_ON) {
-                rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
-                rt2x00mmio_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
-        }
-
-        spin_lock_irqsave(&rt2x00dev->irqmask_lock, flags);
-        reg = 0;
-        if (state == STATE_RADIO_IRQ_ON) {
-                rt2x00_set_field32(&reg, INT_MASK_CSR_RX_DONE, 1);
-                rt2x00_set_field32(&reg, INT_MASK_CSR_TBTT, 1);
-                rt2x00_set_field32(&reg, INT_MASK_CSR_PRE_TBTT, 1);
-                rt2x00_set_field32(&reg, INT_MASK_CSR_TX_FIFO_STATUS, 1);
-                rt2x00_set_field32(&reg, INT_MASK_CSR_AUTO_WAKEUP, 1);
-        }
-        rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
-        spin_unlock_irqrestore(&rt2x00dev->irqmask_lock, flags);
-
-        if (state == STATE_RADIO_IRQ_OFF) {
-                /*
-                 * Wait for possibly running tasklets to finish.
-                 */
-                tasklet_kill(&rt2x00dev->txstatus_tasklet);
-                tasklet_kill(&rt2x00dev->rxdone_tasklet);
-                tasklet_kill(&rt2x00dev->autowake_tasklet);
-                tasklet_kill(&rt2x00dev->tbtt_tasklet);
-                tasklet_kill(&rt2x00dev->pretbtt_tasklet);
-        }
-}
-
-static int rt2800pci_init_registers(struct rt2x00_dev *rt2x00dev)
-{
-        u32 reg;
-
-        /*
-         * Reset DMA indexes
-         */
-        rt2x00mmio_register_read(rt2x00dev, WPDMA_RST_IDX, &reg);
-        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX0, 1);
-        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX1, 1);
-        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX2, 1);
-        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX3, 1);
-        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX4, 1);
-        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DTX_IDX5, 1);
-        rt2x00_set_field32(&reg, WPDMA_RST_IDX_DRX_IDX0, 1);
-        rt2x00mmio_register_write(rt2x00dev, WPDMA_RST_IDX, reg);
-
-        rt2x00mmio_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000e1f);
-        rt2x00mmio_register_write(rt2x00dev, PBF_SYS_CTRL, 0x00000e00);
-
-        if (rt2x00_is_pcie(rt2x00dev) &&
-            (rt2x00_rt(rt2x00dev, RT3090) ||
-             rt2x00_rt(rt2x00dev, RT3390) ||
-             rt2x00_rt(rt2x00dev, RT3572) ||
-             rt2x00_rt(rt2x00dev, RT3593) ||
-             rt2x00_rt(rt2x00dev, RT5390) ||
-             rt2x00_rt(rt2x00dev, RT5392) ||
-             rt2x00_rt(rt2x00dev, RT5592))) {
-                rt2x00mmio_register_read(rt2x00dev, AUX_CTRL, &reg);
-                rt2x00_set_field32(&reg, AUX_CTRL_FORCE_PCIE_CLK, 1);
-                rt2x00_set_field32(&reg, AUX_CTRL_WAKE_PCIE_EN, 1);
-                rt2x00mmio_register_write(rt2x00dev, AUX_CTRL, reg);
-        }
-
-        rt2x00mmio_register_write(rt2x00dev, PWR_PIN_CFG, 0x00000003);
-
-        reg = 0;
-        rt2x00_set_field32(&reg, MAC_SYS_CTRL_RESET_CSR, 1);
-        rt2x00_set_field32(&reg, MAC_SYS_CTRL_RESET_BBP, 1);
-        rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, reg);
-
-        rt2x00mmio_register_write(rt2x00dev, MAC_SYS_CTRL, 0x00000000);
-
-        return 0;
-}
-
 static int rt2800pci_enable_radio(struct rt2x00_dev *rt2x00dev)
 {
         int retval;
 
-        /* Wait for DMA, ignore error until we initialize queues. */
-        rt2800_wait_wpdma_ready(rt2x00dev);
-
-        if (unlikely(rt2800pci_init_queues(rt2x00dev)))
-                return -EIO;
-
-        retval = rt2800_enable_radio(rt2x00dev);
+        retval = rt2800mmio_enable_radio(rt2x00dev);
         if (retval)
                 return retval;
 
@@ -559,15 +227,6 @@ static int rt2800pci_enable_radio(struct rt2x00_dev *rt2x00dev)
         return retval;
 }
 
-static void rt2800pci_disable_radio(struct rt2x00_dev *rt2x00dev)
-{
-        if (rt2x00_is_soc(rt2x00dev)) {
-                rt2800_disable_radio(rt2x00dev);
-                rt2x00mmio_register_write(rt2x00dev, PWR_PIN_CFG, 0);
-                rt2x00mmio_register_write(rt2x00dev, TX_PIN_CFG, 0);
-        }
-}
-
 static int rt2800pci_set_state(struct rt2x00_dev *rt2x00dev,
                                enum dev_state state)
 {
@@ -601,12 +260,11 @@ static int rt2800pci_set_device_state(struct rt2x00_dev *rt2x00dev,
                  * After the radio has been disabled, the device should
                  * be put to sleep for powersaving.
                  */
-                rt2800pci_disable_radio(rt2x00dev);
                 rt2800pci_set_state(rt2x00dev, STATE_SLEEP);
                 break;
         case STATE_RADIO_IRQ_ON:
         case STATE_RADIO_IRQ_OFF:
-                rt2800pci_toggle_irq(rt2x00dev, state);
+                rt2800mmio_toggle_irq(rt2x00dev, state);
                 break;
         case STATE_DEEP_SLEEP:
         case STATE_SLEEP:
@@ -627,479 +285,13 @@ static int rt2800pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 }
 
 /*
- * TX descriptor initialization
- */
-static __le32 *rt2800pci_get_txwi(struct queue_entry *entry)
-{
-        return (__le32 *) entry->skb->data;
-}
-
-static void rt2800pci_write_tx_desc(struct queue_entry *entry,
-                                    struct txentry_desc *txdesc)
-{
-        struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
-        struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
-        __le32 *txd = entry_priv->desc;
-        u32 word;
-        const unsigned int txwi_size = entry->queue->winfo_size;
-
-        /*
-         * The buffers pointed by SD_PTR0/SD_LEN0 and SD_PTR1/SD_LEN1
-         * must contains a TXWI structure + 802.11 header + padding + 802.11
-         * data. We choose to have SD_PTR0/SD_LEN0 only contains TXWI and
-         * SD_PTR1/SD_LEN1 contains 802.11 header + padding + 802.11
-         * data. It means that LAST_SEC0 is always 0.
-         */
-
-        /*
-         * Initialize TX descriptor
-         */
-        word = 0;
-        rt2x00_set_field32(&word, TXD_W0_SD_PTR0, skbdesc->skb_dma);
-        rt2x00_desc_write(txd, 0, word);
-
-        word = 0;
-        rt2x00_set_field32(&word, TXD_W1_SD_LEN1, entry->skb->len);
-        rt2x00_set_field32(&word, TXD_W1_LAST_SEC1,
-                           !test_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags));
-        rt2x00_set_field32(&word, TXD_W1_BURST,
-                           test_bit(ENTRY_TXD_BURST, &txdesc->flags));
-        rt2x00_set_field32(&word, TXD_W1_SD_LEN0, txwi_size);
-        rt2x00_set_field32(&word, TXD_W1_LAST_SEC0, 0);
-        rt2x00_set_field32(&word, TXD_W1_DMA_DONE, 0);
-        rt2x00_desc_write(txd, 1, word);
-
-        word = 0;
-        rt2x00_set_field32(&word, TXD_W2_SD_PTR1,
-                           skbdesc->skb_dma + txwi_size);
-        rt2x00_desc_write(txd, 2, word);
-
-        word = 0;
-        rt2x00_set_field32(&word, TXD_W3_WIV,
-                           !test_bit(ENTRY_TXD_ENCRYPT_IV, &txdesc->flags));
-        rt2x00_set_field32(&word, TXD_W3_QSEL, 2);
-        rt2x00_desc_write(txd, 3, word);
-
-        /*
-         * Register descriptor details in skb frame descriptor.
-         */
-        skbdesc->desc = txd;
-        skbdesc->desc_len = TXD_DESC_SIZE;
-}
-
-/*
- * RX control handlers
- */
-static void rt2800pci_fill_rxdone(struct queue_entry *entry,
-                                  struct rxdone_entry_desc *rxdesc)
-{
-        struct queue_entry_priv_mmio *entry_priv = entry->priv_data;
-        __le32 *rxd = entry_priv->desc;
-        u32 word;
-
-        rt2x00_desc_read(rxd, 3, &word);
-
-        if (rt2x00_get_field32(word, RXD_W3_CRC_ERROR))
-                rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
-
-        /*
-         * Unfortunately we don't know the cipher type used during
-         * decryption. This prevents us from correct providing
-         * correct statistics through debugfs.
-         */
-        rxdesc->cipher_status = rt2x00_get_field32(word, RXD_W3_CIPHER_ERROR);
-
-        if (rt2x00_get_field32(word, RXD_W3_DECRYPTED)) {
-                /*
-                 * Hardware has stripped IV/EIV data from 802.11 frame during
-                 * decryption. Unfortunately the descriptor doesn't contain
-                 * any fields with the EIV/IV data either, so they can't
-                 * be restored by rt2x00lib.
-                 */
-                rxdesc->flags |= RX_FLAG_IV_STRIPPED;
-
-                /*
-                 * The hardware has already checked the Michael Mic and has
-                 * stripped it from the frame. Signal this to mac80211.
-                 */
-                rxdesc->flags |= RX_FLAG_MMIC_STRIPPED;
-
-                if (rxdesc->cipher_status == RX_CRYPTO_SUCCESS)
-                        rxdesc->flags |= RX_FLAG_DECRYPTED;
-                else if (rxdesc->cipher_status == RX_CRYPTO_FAIL_MIC)
-                        rxdesc->flags |= RX_FLAG_MMIC_ERROR;
-        }
-
-        if (rt2x00_get_field32(word, RXD_W3_MY_BSS))
-                rxdesc->dev_flags |= RXDONE_MY_BSS;
-
-        if (rt2x00_get_field32(word, RXD_W3_L2PAD))
-                rxdesc->dev_flags |= RXDONE_L2PAD;
-
-        /*
-         * Process the RXWI structure that is at the start of the buffer.
-         */
-        rt2800_process_rxwi(entry, rxdesc);
-}
-
-/*
- * Interrupt functions.
- */
-static void rt2800pci_wakeup(struct rt2x00_dev *rt2x00dev)
-{
-        struct ieee80211_conf conf = { .flags = 0 };
-        struct rt2x00lib_conf libconf = { .conf = &conf };
-
-        rt2800_config(rt2x00dev, &libconf, IEEE80211_CONF_CHANGE_PS);
-}
-
-static bool rt2800pci_txdone_entry_check(struct queue_entry *entry, u32 status)
-{
-        __le32 *txwi;
-        u32 word;
-        int wcid, tx_wcid;
-
-        wcid = rt2x00_get_field32(status, TX_STA_FIFO_WCID);
-
-        txwi = rt2800_drv_get_txwi(entry);
-        rt2x00_desc_read(txwi, 1, &word);
-        tx_wcid = rt2x00_get_field32(word, TXWI_W1_WIRELESS_CLI_ID);
-
-        return (tx_wcid == wcid);
-}
-
-static bool rt2800pci_txdone_find_entry(struct queue_entry *entry, void *data)
-{
-        u32 status = *(u32 *)data;
-
-        /*
-         * rt2800pci hardware might reorder frames when exchanging traffic
-         * with multiple BA enabled STAs.
-         *
-         * For example, a tx queue
-         *    [ STA1 | STA2 | STA1 | STA2 ]
-         * can result in tx status reports
-         *    [ STA1 | STA1 | STA2 | STA2 ]
-         * when the hw decides to aggregate the frames for STA1 into one AMPDU.
-         *
-         * To mitigate this effect, associate the tx status to the first frame
-         * in the tx queue with a matching wcid.
-         */
-        if (rt2800pci_txdone_entry_check(entry, status) &&
-            !test_bit(ENTRY_DATA_STATUS_SET, &entry->flags)) {
-                /*
-                 * Got a matching frame, associate the tx status with
-                 * the frame
-                 */
-                entry->status = status;
-                set_bit(ENTRY_DATA_STATUS_SET, &entry->flags);
-                return true;
-        }
-
-        /* Check the next frame */
-        return false;
-}
-
-static bool rt2800pci_txdone_match_first(struct queue_entry *entry, void *data)
-{
-        u32 status = *(u32 *)data;
-
-        /*
-         * Find the first frame without tx status and assign this status to it
-         * regardless if it matches or not.
-         */
-        if (!test_bit(ENTRY_DATA_STATUS_SET, &entry->flags)) {
-                /*
-                 * Got a matching frame, associate the tx status with
-                 * the frame
-                 */
-                entry->status = status;
-                set_bit(ENTRY_DATA_STATUS_SET, &entry->flags);
-                return true;
-        }
-
-        /* Check the next frame */
-        return false;
-}
-static bool rt2800pci_txdone_release_entries(struct queue_entry *entry,
-                                             void *data)
-{
-        if (test_bit(ENTRY_DATA_STATUS_SET, &entry->flags)) {
-                rt2800_txdone_entry(entry, entry->status,
-                                    rt2800pci_get_txwi(entry));
-                return false;
-        }
-
-        /* No more frames to release */
-        return true;
-}
-
-static bool rt2800pci_txdone(struct rt2x00_dev *rt2x00dev)
-{
-        struct data_queue *queue;
-        u32 status;
-        u8 qid;
-        int max_tx_done = 16;
-
-        while (kfifo_get(&rt2x00dev->txstatus_fifo, &status)) {
-                qid = rt2x00_get_field32(status, TX_STA_FIFO_PID_QUEUE);
-                if (unlikely(qid >= QID_RX)) {
-                        /*
-                         * Unknown queue, this shouldn't happen. Just drop
-                         * this tx status.
-                         */
-                        rt2x00_warn(rt2x00dev, "Got TX status report with unexpected pid %u, dropping\n",
-                                    qid);
-                        break;
-                }
-
-                queue = rt2x00queue_get_tx_queue(rt2x00dev, qid);
-                if (unlikely(queue == NULL)) {
-                        /*
-                         * The queue is NULL, this shouldn't happen. Stop
-                         * processing here and drop the tx status
-                         */
-                        rt2x00_warn(rt2x00dev, "Got TX status for an unavailable queue %u, dropping\n",
-                                    qid);
-                        break;
-                }
-
-                if (unlikely(rt2x00queue_empty(queue))) {
-                        /*
-                         * The queue is empty. Stop processing here
-                         * and drop the tx status.
-                         */
-                        rt2x00_warn(rt2x00dev, "Got TX status for an empty queue %u, dropping\n",
-                                    qid);
-                        break;
-                }
-
-                /*
-                 * Let's associate this tx status with the first
-                 * matching frame.
-                 */
-                if (!rt2x00queue_for_each_entry(queue, Q_INDEX_DONE,
-                                                Q_INDEX, &status,
-                                                rt2800pci_txdone_find_entry)) {
-                        /*
-                         * We cannot match the tx status to any frame, so just
-                         * use the first one.
-                         */
-                        if (!rt2x00queue_for_each_entry(queue, Q_INDEX_DONE,
-                                                        Q_INDEX, &status,
-                                                        rt2800pci_txdone_match_first)) {
-                                rt2x00_warn(rt2x00dev, "No frame found for TX status on queue %u, dropping\n",
-                                            qid);
-                                break;
-                        }
-                }
-
-                /*
-                 * Release all frames with a valid tx status.
-                 */
-                rt2x00queue_for_each_entry(queue, Q_INDEX_DONE,
-                                           Q_INDEX, NULL,
-                                           rt2800pci_txdone_release_entries);
-
-                if (--max_tx_done == 0)
-                        break;
-        }
-
-        return !max_tx_done;
-}
-
-static inline void rt2800pci_enable_interrupt(struct rt2x00_dev *rt2x00dev,
-                                              struct rt2x00_field32 irq_field)
-{
-        u32 reg;
-
-        /*
-         * Enable a single interrupt. The interrupt mask register
-         * access needs locking.
-         */
-        spin_lock_irq(&rt2x00dev->irqmask_lock);
-        rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
-        rt2x00_set_field32(&reg, irq_field, 1);
-        rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
-        spin_unlock_irq(&rt2x00dev->irqmask_lock);
-}
-
-static void rt2800pci_txstatus_tasklet(unsigned long data)
-{
-        struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
-        if (rt2800pci_txdone(rt2x00dev))
-                tasklet_schedule(&rt2x00dev->txstatus_tasklet);
-
-        /*
-         * No need to enable the tx status interrupt here as we always
-         * leave it enabled to minimize the possibility of a tx status
-         * register overflow. See comment in interrupt handler.
-         */
-}
-
-static void rt2800pci_pretbtt_tasklet(unsigned long data)
-{
-        struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
-        rt2x00lib_pretbtt(rt2x00dev);
-        if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
-                rt2800pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_PRE_TBTT);
-}
-
-static void rt2800pci_tbtt_tasklet(unsigned long data)
-{
-        struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
-        struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
-        u32 reg;
-
-        rt2x00lib_beacondone(rt2x00dev);
-
-        if (rt2x00dev->intf_ap_count) {
-                /*
-                 * The rt2800pci hardware tbtt timer is off by 1us per tbtt
-                 * causing beacon skew and as a result causing problems with
-                 * some powersaving clients over time. Shorten the beacon
-                 * interval every 64 beacons by 64us to mitigate this effect.
-                 */
-                if (drv_data->tbtt_tick == (BCN_TBTT_OFFSET - 2)) {
-                        rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
-                        rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_INTERVAL,
-                                           (rt2x00dev->beacon_int * 16) - 1);
-                        rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
-                } else if (drv_data->tbtt_tick == (BCN_TBTT_OFFSET - 1)) {
-                        rt2x00mmio_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
-                        rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_INTERVAL,
-                                           (rt2x00dev->beacon_int * 16));
-                        rt2x00mmio_register_write(rt2x00dev, BCN_TIME_CFG, reg);
-                }
-                drv_data->tbtt_tick++;
-                drv_data->tbtt_tick %= BCN_TBTT_OFFSET;
-        }
-
-        if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
-                rt2800pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_TBTT);
-}
-
-static void rt2800pci_rxdone_tasklet(unsigned long data)
-{
-        struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
-        if (rt2x00mmio_rxdone(rt2x00dev))
-                tasklet_schedule(&rt2x00dev->rxdone_tasklet);
-        else if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
-                rt2800pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_RX_DONE);
-}
-
-static void rt2800pci_autowake_tasklet(unsigned long data)
-{
-        struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
-        rt2800pci_wakeup(rt2x00dev);
-        if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
-                rt2800pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_AUTO_WAKEUP);
-}
-
-static void rt2800pci_txstatus_interrupt(struct rt2x00_dev *rt2x00dev)
-{
-        u32 status;
-        int i;
-
-        /*
-         * The TX_FIFO_STATUS interrupt needs special care. We should
-         * read TX_STA_FIFO but we should do it immediately as otherwise
-         * the register can overflow and we would lose status reports.
-         *
-         * Hence, read the TX_STA_FIFO register and copy all tx status
-         * reports into a kernel FIFO which is handled in the txstatus
-         * tasklet. We use a tasklet to process the tx status reports
-         * because we can schedule the tasklet multiple times (when the
-         * interrupt fires again during tx status processing).
-         *
-         * Furthermore we don't disable the TX_FIFO_STATUS
-         * interrupt here but leave it enabled so that the TX_STA_FIFO
-         * can also be read while the tx status tasklet gets executed.
-         *
-         * Since we have only one producer and one consumer we don't
-         * need to lock the kfifo.
-         */
-        for (i = 0; i < rt2x00dev->tx->limit; i++) {
-                rt2x00mmio_register_read(rt2x00dev, TX_STA_FIFO, &status);
-
-                if (!rt2x00_get_field32(status, TX_STA_FIFO_VALID))
-                        break;
-
-                if (!kfifo_put(&rt2x00dev->txstatus_fifo, &status)) {
-                        rt2x00_warn(rt2x00dev, "TX status FIFO overrun, drop tx status report\n");
-                        break;
-                }
-        }
-
-        /* Schedule the tasklet for processing the tx status. */
-        tasklet_schedule(&rt2x00dev->txstatus_tasklet);
-}
-
-static irqreturn_t rt2800pci_interrupt(int irq, void *dev_instance)
-{
-        struct rt2x00_dev *rt2x00dev = dev_instance;
-        u32 reg, mask;
-
-        /* Read status and ACK all interrupts */
-        rt2x00mmio_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
-        rt2x00mmio_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
-
-        if (!reg)
-                return IRQ_NONE;
-
-        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
-                return IRQ_HANDLED;
-
-        /*
-         * Since INT_MASK_CSR and INT_SOURCE_CSR use the same bits
-         * for interrupts and interrupt masks we can just use the value of
-         * INT_SOURCE_CSR to create the interrupt mask.
-         */
-        mask = ~reg;
-
-        if (rt2x00_get_field32(reg, INT_SOURCE_CSR_TX_FIFO_STATUS)) {
-                rt2800pci_txstatus_interrupt(rt2x00dev);
-                /*
-                 * Never disable the TX_FIFO_STATUS interrupt.
-                 */
-                rt2x00_set_field32(&mask, INT_MASK_CSR_TX_FIFO_STATUS, 1);
-        }
-
-        if (rt2x00_get_field32(reg, INT_SOURCE_CSR_PRE_TBTT))
-                tasklet_hi_schedule(&rt2x00dev->pretbtt_tasklet);
-
-        if (rt2x00_get_field32(reg, INT_SOURCE_CSR_TBTT))
-                tasklet_hi_schedule(&rt2x00dev->tbtt_tasklet);
-
-        if (rt2x00_get_field32(reg, INT_SOURCE_CSR_RX_DONE))
-                tasklet_schedule(&rt2x00dev->rxdone_tasklet);
-
-        if (rt2x00_get_field32(reg, INT_SOURCE_CSR_AUTO_WAKEUP))
-                tasklet_schedule(&rt2x00dev->autowake_tasklet);
-
-        /*
-         * Disable all interrupts for which a tasklet was scheduled right now,
-         * the tasklet will reenable the appropriate interrupts.
-         */
-        spin_lock(&rt2x00dev->irqmask_lock);
-        rt2x00mmio_register_read(rt2x00dev, INT_MASK_CSR, &reg);
-        reg &= mask;
-        rt2x00mmio_register_write(rt2x00dev, INT_MASK_CSR, reg);
-        spin_unlock(&rt2x00dev->irqmask_lock);
-
-        return IRQ_HANDLED;
-}
-
-/*
  * Device probe functions.
  */
 static int rt2800pci_read_eeprom(struct rt2x00_dev *rt2x00dev)
 {
         int retval;
 
-        if (rt2x00_is_soc(rt2x00dev))
-                retval = rt2800pci_read_eeprom_soc(rt2x00dev);
-        else if (rt2800pci_efuse_detect(rt2x00dev))
+        if (rt2800pci_efuse_detect(rt2x00dev))
                 retval = rt2800pci_read_eeprom_efuse(rt2x00dev);
         else
                 retval = rt2800pci_read_eeprom_pci(rt2x00dev);
@@ -1145,25 +337,25 @@ static const struct rt2800_ops rt2800pci_rt2800_ops = {
         .read_eeprom            = rt2800pci_read_eeprom,
         .hwcrypt_disabled       = rt2800pci_hwcrypt_disabled,
         .drv_write_firmware     = rt2800pci_write_firmware,
-        .drv_init_registers     = rt2800pci_init_registers,
-        .drv_get_txwi           = rt2800pci_get_txwi,
+        .drv_init_registers     = rt2800mmio_init_registers,
+        .drv_get_txwi           = rt2800mmio_get_txwi,
 };
 
 static const struct rt2x00lib_ops rt2800pci_rt2x00_ops = {
-        .irq_handler            = rt2800pci_interrupt,
-        .txstatus_tasklet       = rt2800pci_txstatus_tasklet,
-        .pretbtt_tasklet        = rt2800pci_pretbtt_tasklet,
-        .tbtt_tasklet           = rt2800pci_tbtt_tasklet,
-        .rxdone_tasklet         = rt2800pci_rxdone_tasklet,
-        .autowake_tasklet       = rt2800pci_autowake_tasklet,
+        .irq_handler            = rt2800mmio_interrupt,
+        .txstatus_tasklet       = rt2800mmio_txstatus_tasklet,
+        .pretbtt_tasklet        = rt2800mmio_pretbtt_tasklet,
+        .tbtt_tasklet           = rt2800mmio_tbtt_tasklet,
+        .rxdone_tasklet         = rt2800mmio_rxdone_tasklet,
+        .autowake_tasklet       = rt2800mmio_autowake_tasklet,
         .probe_hw               = rt2800_probe_hw,
         .get_firmware_name      = rt2800pci_get_firmware_name,
         .check_firmware         = rt2800_check_firmware,
         .load_firmware          = rt2800_load_firmware,
         .initialize             = rt2x00mmio_initialize,
         .uninitialize           = rt2x00mmio_uninitialize,
-        .get_entry_state        = rt2800pci_get_entry_state,
-        .clear_entry            = rt2800pci_clear_entry,
+        .get_entry_state        = rt2800mmio_get_entry_state,
+        .clear_entry            = rt2800mmio_clear_entry,
         .set_device_state       = rt2800pci_set_device_state,
         .rfkill_poll            = rt2800_rfkill_poll,
         .link_stats             = rt2800_link_stats,
@@ -1171,15 +363,15 @@ static const struct rt2x00lib_ops rt2800pci_rt2x00_ops = {
         .link_tuner             = rt2800_link_tuner,
         .gain_calibration       = rt2800_gain_calibration,
         .vco_calibration        = rt2800_vco_calibration,
-        .start_queue            = rt2800pci_start_queue,
-        .kick_queue             = rt2800pci_kick_queue,
-        .stop_queue             = rt2800pci_stop_queue,
+        .start_queue            = rt2800mmio_start_queue,
+        .kick_queue             = rt2800mmio_kick_queue,
+        .stop_queue             = rt2800mmio_stop_queue,
         .flush_queue            = rt2x00mmio_flush_queue,
-        .write_tx_desc          = rt2800pci_write_tx_desc,
+        .write_tx_desc          = rt2800mmio_write_tx_desc,
         .write_tx_data          = rt2800_write_tx_data,
         .write_beacon           = rt2800_write_beacon,
         .clear_beacon           = rt2800_clear_beacon,
-        .fill_rxdone            = rt2800pci_fill_rxdone,
+        .fill_rxdone            = rt2800mmio_fill_rxdone,
         .config_shared_key      = rt2800_config_shared_key,
         .config_pairwise_key    = rt2800_config_pairwise_key,
         .config_filter          = rt2800_config_filter,
@@ -1191,49 +383,6 @@ static const struct rt2x00lib_ops rt2800pci_rt2x00_ops = {
         .sta_remove             = rt2800_sta_remove,
 };
 
-static void rt2800pci_queue_init(struct data_queue *queue)
-{
-        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
-        unsigned short txwi_size, rxwi_size;
-
-        rt2800_get_txwi_rxwi_size(rt2x00dev, &txwi_size, &rxwi_size);
-
-        switch (queue->qid) {
-        case QID_RX:
-                queue->limit = 128;
-                queue->data_size = AGGREGATION_SIZE;
-                queue->desc_size = RXD_DESC_SIZE;
-                queue->winfo_size = rxwi_size;
-                queue->priv_size = sizeof(struct queue_entry_priv_mmio);
-                break;
-
-        case QID_AC_VO:
-        case QID_AC_VI:
-        case QID_AC_BE:
-        case QID_AC_BK:
-                queue->limit = 64;
-                queue->data_size = AGGREGATION_SIZE;
-                queue->desc_size = TXD_DESC_SIZE;
-                queue->winfo_size = txwi_size;
-                queue->priv_size = sizeof(struct queue_entry_priv_mmio);
-                break;
-
-        case QID_BEACON:
-                queue->limit = 8;
-                queue->data_size = 0; /* No DMA required for beacons */
-                queue->desc_size = TXD_DESC_SIZE;
-                queue->winfo_size = txwi_size;
-                queue->priv_size = sizeof(struct queue_entry_priv_mmio);
-                break;
-
-        case QID_ATIM:
-                /* fallthrough */
-        default:
-                BUG();
-                break;
-        }
-}
-
 static const struct rt2x00_ops rt2800pci_ops = {
         .name                   = KBUILD_MODNAME,
         .drv_data_size          = sizeof(struct rt2800_drv_data),
@@ -1241,7 +390,7 @@ static const struct rt2x00_ops rt2800pci_ops = {
         .eeprom_size            = EEPROM_SIZE,
         .rf_size                = RF_SIZE,
         .tx_queues              = NUM_TX_QUEUES,
-        .queue_init             = rt2800pci_queue_init,
+        .queue_init             = rt2800mmio_queue_init,
         .lib                    = &rt2800pci_rt2x00_ops,
         .drv                    = &rt2800pci_rt2800_ops,
         .hw                     = &rt2800pci_mac80211_ops,
@@ -1253,7 +402,6 @@ static const struct rt2x00_ops rt2800pci_ops = {
 /*
  * RT2800pci module information.
  */
-#ifdef CONFIG_PCI
 static DEFINE_PCI_DEVICE_TABLE(rt2800pci_device_table) = {
         { PCI_DEVICE(0x1814, 0x0601) },
         { PCI_DEVICE(0x1814, 0x0681) },
@@ -1298,38 +446,15 @@ static DEFINE_PCI_DEVICE_TABLE(rt2800pci_device_table) = {
 #endif
         { 0, }
 };
-#endif /* CONFIG_PCI */
 
 MODULE_AUTHOR(DRV_PROJECT);
 MODULE_VERSION(DRV_VERSION);
 MODULE_DESCRIPTION("Ralink RT2800 PCI & PCMCIA Wireless LAN driver.");
 MODULE_SUPPORTED_DEVICE("Ralink RT2860 PCI & PCMCIA chipset based cards");
-#ifdef CONFIG_PCI
 MODULE_FIRMWARE(FIRMWARE_RT2860);
 MODULE_DEVICE_TABLE(pci, rt2800pci_device_table);
-#endif /* CONFIG_PCI */
 MODULE_LICENSE("GPL");
 
-#if defined(CONFIG_SOC_RT288X) || defined(CONFIG_SOC_RT305X)
-static int rt2800soc_probe(struct platform_device *pdev)
-{
-        return rt2x00soc_probe(pdev, &rt2800pci_ops);
-}
-
-static struct platform_driver rt2800soc_driver = {
-        .driver         = {
-                .name           = "rt2800_wmac",
-                .owner          = THIS_MODULE,
-                .mod_name       = KBUILD_MODNAME,
-        },
-        .probe          = rt2800soc_probe,
-        .remove         = rt2x00soc_remove,
-        .suspend        = rt2x00soc_suspend,
-        .resume         = rt2x00soc_resume,
-};
-#endif /* CONFIG_SOC_RT288X || CONFIG_SOC_RT305X */
-
-#ifdef CONFIG_PCI
 static int rt2800pci_probe(struct pci_dev *pci_dev,
                            const struct pci_device_id *id)
 {
@@ -1344,39 +469,5 @@ static struct pci_driver rt2800pci_driver = {
         .suspend        = rt2x00pci_suspend,
         .resume         = rt2x00pci_resume,
 };
-#endif /* CONFIG_PCI */
-
-static int __init rt2800pci_init(void)
-{
-        int ret = 0;
-
-#if defined(CONFIG_SOC_RT288X) || defined(CONFIG_SOC_RT305X)
-        ret = platform_driver_register(&rt2800soc_driver);
-        if (ret)
-                return ret;
-#endif
-#ifdef CONFIG_PCI
-        ret = pci_register_driver(&rt2800pci_driver);
-        if (ret) {
-#if defined(CONFIG_SOC_RT288X) || defined(CONFIG_SOC_RT305X)
-                platform_driver_unregister(&rt2800soc_driver);
-#endif
-                return ret;
-        }
-#endif
-
-        return ret;
-}
-
-static void __exit rt2800pci_exit(void)
-{
-#ifdef CONFIG_PCI
-        pci_unregister_driver(&rt2800pci_driver);
-#endif
-#if defined(CONFIG_SOC_RT288X) || defined(CONFIG_SOC_RT305X)
-        platform_driver_unregister(&rt2800soc_driver);
-#endif
-}
 
-module_init(rt2800pci_init);
-module_exit(rt2800pci_exit);
+module_pci_driver(rt2800pci_driver);
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.h b/drivers/net/wireless/rt2x00/rt2800pci.h
index ab22a087c50d..a81c9ee281c0 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.h
+++ b/drivers/net/wireless/rt2x00/rt2800pci.h
@@ -35,107 +35,10 @@
 #define RT2800PCI_H
 
 /*
- * Queue register offset macros
- */
-#define TX_QUEUE_REG_OFFSET             0x10
-#define TX_BASE_PTR(__x)                (TX_BASE_PTR0 + ((__x) * TX_QUEUE_REG_OFFSET))
-#define TX_MAX_CNT(__x)                 (TX_MAX_CNT0 + ((__x) * TX_QUEUE_REG_OFFSET))
-#define TX_CTX_IDX(__x)                 (TX_CTX_IDX0 + ((__x) * TX_QUEUE_REG_OFFSET))
-#define TX_DTX_IDX(__x)                 (TX_DTX_IDX0 + ((__x) * TX_QUEUE_REG_OFFSET))
-
-/*
  * 8051 firmware image.
  */
 #define FIRMWARE_RT2860                 "rt2860.bin"
 #define FIRMWARE_RT3290                 "rt3290.bin"
 #define FIRMWARE_IMAGE_BASE             0x2000
 
-/*
- * DMA descriptor defines.
- */
-#define TXD_DESC_SIZE                   (4 * sizeof(__le32))
-#define RXD_DESC_SIZE                   (4 * sizeof(__le32))
-
-/*
- * TX descriptor format for TX, PRIO and Beacon Ring.
- */
-
-/*
- * Word0
- */
-#define TXD_W0_SD_PTR0                  FIELD32(0xffffffff)
-
-/*
- * Word1
- */
-#define TXD_W1_SD_LEN1                  FIELD32(0x00003fff)
-#define TXD_W1_LAST_SEC1                FIELD32(0x00004000)
-#define TXD_W1_BURST                    FIELD32(0x00008000)
-#define TXD_W1_SD_LEN0                  FIELD32(0x3fff0000)
-#define TXD_W1_LAST_SEC0                FIELD32(0x40000000)
-#define TXD_W1_DMA_DONE                 FIELD32(0x80000000)
-
-/*
- * Word2
- */
-#define TXD_W2_SD_PTR1                  FIELD32(0xffffffff)
-
-/*
- * Word3
- * WIV: Wireless Info Valid. 1: Driver filled WI, 0: DMA needs to copy WI
- * QSEL: Select on-chip FIFO ID for 2nd-stage output scheduler.
- *       0:MGMT, 1:HCCA 2:EDCA
- */
-#define TXD_W3_WIV                      FIELD32(0x01000000)
-#define TXD_W3_QSEL                     FIELD32(0x06000000)
-#define TXD_W3_TCO                      FIELD32(0x20000000)
-#define TXD_W3_UCO                      FIELD32(0x40000000)
-#define TXD_W3_ICO                      FIELD32(0x80000000)
-
-/*
- * RX descriptor format for RX Ring.
- */
-
-/*
- * Word0
- */
-#define RXD_W0_SDP0                     FIELD32(0xffffffff)
-
-/*
- * Word1
- */
-#define RXD_W1_SDL1                     FIELD32(0x00003fff)
-#define RXD_W1_SDL0                     FIELD32(0x3fff0000)
-#define RXD_W1_LS0                      FIELD32(0x40000000)
-#define RXD_W1_DMA_DONE                 FIELD32(0x80000000)
-
-/*
- * Word2
- */
-#define RXD_W2_SDP1                     FIELD32(0xffffffff)
-
-/*
- * Word3
- * AMSDU: RX with 802.3 header, not 802.11 header.
- * DECRYPTED: This frame is being decrypted.
- */
-#define RXD_W3_BA                       FIELD32(0x00000001)
-#define RXD_W3_DATA                     FIELD32(0x00000002)
-#define RXD_W3_NULLDATA                 FIELD32(0x00000004)
-#define RXD_W3_FRAG                     FIELD32(0x00000008)
-#define RXD_W3_UNICAST_TO_ME            FIELD32(0x00000010)
-#define RXD_W3_MULTICAST                FIELD32(0x00000020)
-#define RXD_W3_BROADCAST                FIELD32(0x00000040)
-#define RXD_W3_MY_BSS                   FIELD32(0x00000080)
-#define RXD_W3_CRC_ERROR                FIELD32(0x00000100)
-#define RXD_W3_CIPHER_ERROR             FIELD32(0x00000600)
-#define RXD_W3_AMSDU                    FIELD32(0x00000800)
-#define RXD_W3_HTC                      FIELD32(0x00001000)
-#define RXD_W3_RSSI                     FIELD32(0x00002000)
-#define RXD_W3_L2PAD                    FIELD32(0x00004000)
-#define RXD_W3_AMPDU                    FIELD32(0x00008000)
-#define RXD_W3_DECRYPTED                FIELD32(0x00010000)
-#define RXD_W3_PLCP_SIGNAL              FIELD32(0x00020000)
-#define RXD_W3_PLCP_RSSI                FIELD32(0x00040000)
-
 #endif /* RT2800PCI_H */
diff --git a/drivers/net/wireless/rt2x00/rt2800soc.c b/drivers/net/wireless/rt2x00/rt2800soc.c
new file mode 100644
index 000000000000..1359227ca411
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2800soc.c
@@ -0,0 +1,263 @@
+/*      Copyright (C) 2009 - 2010 Ivo van Doorn <IvDoorn@gmail.com>
+ *      Copyright (C) 2009 Alban Browaeys <prahal@yahoo.com>
+ *      Copyright (C) 2009 Felix Fietkau <nbd@openwrt.org>
+ *      Copyright (C) 2009 Luis Correia <luis.f.correia@gmail.com>
+ *      Copyright (C) 2009 Mattias Nissler <mattias.nissler@gmx.de>
+ *      Copyright (C) 2009 Mark Asselstine <asselsm@gmail.com>
+ *      Copyright (C) 2009 Xose Vazquez Perez <xose.vazquez@gmail.com>
+ *      Copyright (C) 2009 Bart Zolnierkiewicz <bzolnier@gmail.com>
+ *      <http://rt2x00.serialmonkey.com>
+ *
+ *      This program is free software; you can redistribute it and/or modify
+ *      it under the terms of the GNU General Public License as published by
+ *      the Free Software Foundation; either version 2 of the License, or
+ *      (at your option) any later version.
+ *
+ *      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.
+ *
+ *      You should have received a copy of the GNU General Public License
+ *      along with this program; if not, write to the
+ *      Free Software Foundation, Inc.,
+ *      59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*      Module: rt2800soc
+ *      Abstract: rt2800 WiSoC specific routines.
+ */
+
+#include <linux/etherdevice.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include "rt2x00.h"
+#include "rt2x00mmio.h"
+#include "rt2x00soc.h"
+#include "rt2800.h"
+#include "rt2800lib.h"
+#include "rt2800mmio.h"
+
+/* Allow hardware encryption to be disabled. */
+static bool modparam_nohwcrypt;
+module_param_named(nohwcrypt, modparam_nohwcrypt, bool, S_IRUGO);
+MODULE_PARM_DESC(nohwcrypt, "Disable hardware encryption.");
+
+static bool rt2800soc_hwcrypt_disabled(struct rt2x00_dev *rt2x00dev)
+{
+        return modparam_nohwcrypt;
+}
+
+static void rt2800soc_disable_radio(struct rt2x00_dev *rt2x00dev)
+{
+        rt2800_disable_radio(rt2x00dev);
+        rt2x00mmio_register_write(rt2x00dev, PWR_PIN_CFG, 0);
+        rt2x00mmio_register_write(rt2x00dev, TX_PIN_CFG, 0);
+}
+
+static int rt2800soc_set_device_state(struct rt2x00_dev *rt2x00dev,
+                                      enum dev_state state)
+{
+        int retval = 0;
+
+        switch (state) {
+        case STATE_RADIO_ON:
+                retval = rt2800mmio_enable_radio(rt2x00dev);
+                break;
+
+        case STATE_RADIO_OFF:
+                rt2800soc_disable_radio(rt2x00dev);
+                break;
+
+        case STATE_RADIO_IRQ_ON:
+        case STATE_RADIO_IRQ_OFF:
+                rt2800mmio_toggle_irq(rt2x00dev, state);
+                break;
+
+        case STATE_DEEP_SLEEP:
+        case STATE_SLEEP:
+        case STATE_STANDBY:
+        case STATE_AWAKE:
+                /* These states are not supported, but don't report an error */
+                retval = 0;
+                break;
+
+        default:
+                retval = -ENOTSUPP;
+                break;
+        }
+
+        if (unlikely(retval))
+                rt2x00_err(rt2x00dev, "Device failed to enter state %d (%d)\n",
+                           state, retval);
+
+        return retval;
+}
+
+static int rt2800soc_read_eeprom(struct rt2x00_dev *rt2x00dev)
+{
+        void __iomem *base_addr = ioremap(0x1F040000, EEPROM_SIZE);
+
+        if (!base_addr)
+                return -ENOMEM;
+
+        memcpy_fromio(rt2x00dev->eeprom, base_addr, EEPROM_SIZE);
+
+        iounmap(base_addr);
+        return 0;
+}
+
+/* Firmware functions */
+static char *rt2800soc_get_firmware_name(struct rt2x00_dev *rt2x00dev)
+{
+        WARN_ON_ONCE(1);
+        return NULL;
+}
+
+static int rt2800soc_load_firmware(struct rt2x00_dev *rt2x00dev,
+                                   const u8 *data, const size_t len)
+{
+        WARN_ON_ONCE(1);
+        return 0;
+}
+
+static int rt2800soc_check_firmware(struct rt2x00_dev *rt2x00dev,
+                                    const u8 *data, const size_t len)
+{
+        WARN_ON_ONCE(1);
+        return 0;
+}
+
+static int rt2800soc_write_firmware(struct rt2x00_dev *rt2x00dev,
+                                    const u8 *data, const size_t len)
+{
+        WARN_ON_ONCE(1);
+        return 0;
+}
+
+static const struct ieee80211_ops rt2800soc_mac80211_ops = {
+        .tx                     = rt2x00mac_tx,
+        .start                  = rt2x00mac_start,
+        .stop                   = rt2x00mac_stop,
+        .add_interface          = rt2x00mac_add_interface,
+        .remove_interface       = rt2x00mac_remove_interface,
+        .config                 = rt2x00mac_config,
+        .configure_filter       = rt2x00mac_configure_filter,
+        .set_key                = rt2x00mac_set_key,
+        .sw_scan_start          = rt2x00mac_sw_scan_start,
+        .sw_scan_complete       = rt2x00mac_sw_scan_complete,
+        .get_stats              = rt2x00mac_get_stats,
+        .get_tkip_seq           = rt2800_get_tkip_seq,
+        .set_rts_threshold      = rt2800_set_rts_threshold,
+        .sta_add                = rt2x00mac_sta_add,
+        .sta_remove             = rt2x00mac_sta_remove,
+        .bss_info_changed       = rt2x00mac_bss_info_changed,
+        .conf_tx                = rt2800_conf_tx,
+        .get_tsf                = rt2800_get_tsf,
+        .rfkill_poll            = rt2x00mac_rfkill_poll,
+        .ampdu_action           = rt2800_ampdu_action,
+        .flush                  = rt2x00mac_flush,
+        .get_survey             = rt2800_get_survey,
+        .get_ringparam          = rt2x00mac_get_ringparam,
+        .tx_frames_pending      = rt2x00mac_tx_frames_pending,
+};
+
+static const struct rt2800_ops rt2800soc_rt2800_ops = {
+        .register_read          = rt2x00mmio_register_read,
+        .register_read_lock     = rt2x00mmio_register_read, /* same for SoCs */
+        .register_write         = rt2x00mmio_register_write,
+        .register_write_lock    = rt2x00mmio_register_write, /* same for SoCs */
+        .register_multiread     = rt2x00mmio_register_multiread,
+        .register_multiwrite    = rt2x00mmio_register_multiwrite,
+        .regbusy_read           = rt2x00mmio_regbusy_read,
+        .read_eeprom            = rt2800soc_read_eeprom,
+        .hwcrypt_disabled       = rt2800soc_hwcrypt_disabled,
+        .drv_write_firmware     = rt2800soc_write_firmware,
+        .drv_init_registers     = rt2800mmio_init_registers,
+        .drv_get_txwi           = rt2800mmio_get_txwi,
+};
+
+static const struct rt2x00lib_ops rt2800soc_rt2x00_ops = {
+        .irq_handler            = rt2800mmio_interrupt,
+        .txstatus_tasklet       = rt2800mmio_txstatus_tasklet,
+        .pretbtt_tasklet        = rt2800mmio_pretbtt_tasklet,
+        .tbtt_tasklet           = rt2800mmio_tbtt_tasklet,
+        .rxdone_tasklet         = rt2800mmio_rxdone_tasklet,
+        .autowake_tasklet       = rt2800mmio_autowake_tasklet,
+        .probe_hw               = rt2800_probe_hw,
+        .get_firmware_name      = rt2800soc_get_firmware_name,
+        .check_firmware         = rt2800soc_check_firmware,
+        .load_firmware          = rt2800soc_load_firmware,
+        .initialize             = rt2x00mmio_initialize,
+        .uninitialize           = rt2x00mmio_uninitialize,
+        .get_entry_state        = rt2800mmio_get_entry_state,
+        .clear_entry            = rt2800mmio_clear_entry,
+        .set_device_state       = rt2800soc_set_device_state,
+        .rfkill_poll            = rt2800_rfkill_poll,
+        .link_stats             = rt2800_link_stats,
+        .reset_tuner            = rt2800_reset_tuner,
+        .link_tuner             = rt2800_link_tuner,
+        .gain_calibration       = rt2800_gain_calibration,
+        .vco_calibration        = rt2800_vco_calibration,
+        .start_queue            = rt2800mmio_start_queue,
+        .kick_queue             = rt2800mmio_kick_queue,
+        .stop_queue             = rt2800mmio_stop_queue,
+        .flush_queue            = rt2x00mmio_flush_queue,
+        .write_tx_desc          = rt2800mmio_write_tx_desc,
+        .write_tx_data          = rt2800_write_tx_data,
+        .write_beacon           = rt2800_write_beacon,
+        .clear_beacon           = rt2800_clear_beacon,
+        .fill_rxdone            = rt2800mmio_fill_rxdone,
+        .config_shared_key      = rt2800_config_shared_key,
+        .config_pairwise_key    = rt2800_config_pairwise_key,
+        .config_filter          = rt2800_config_filter,
+        .config_intf            = rt2800_config_intf,
+        .config_erp             = rt2800_config_erp,
+        .config_ant             = rt2800_config_ant,
+        .config                 = rt2800_config,
+        .sta_add                = rt2800_sta_add,
+        .sta_remove             = rt2800_sta_remove,
+};
+
+static const struct rt2x00_ops rt2800soc_ops = {
+        .name                   = KBUILD_MODNAME,
+        .drv_data_size          = sizeof(struct rt2800_drv_data),
+        .max_ap_intf            = 8,
+        .eeprom_size            = EEPROM_SIZE,
+        .rf_size                = RF_SIZE,
+        .tx_queues              = NUM_TX_QUEUES,
+        .queue_init             = rt2800mmio_queue_init,
+        .lib                    = &rt2800soc_rt2x00_ops,
+        .drv                    = &rt2800soc_rt2800_ops,
+        .hw                     = &rt2800soc_mac80211_ops,
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+        .debugfs                = &rt2800_rt2x00debug,
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+};
+
+static int rt2800soc_probe(struct platform_device *pdev)
+{
+        return rt2x00soc_probe(pdev, &rt2800soc_ops);
+}
+
+static struct platform_driver rt2800soc_driver = {
+        .driver         = {
+                .name           = "rt2800_wmac",
+                .owner          = THIS_MODULE,
+                .mod_name       = KBUILD_MODNAME,
+        },
+        .probe          = rt2800soc_probe,
+        .remove         = rt2x00soc_remove,
+        .suspend        = rt2x00soc_suspend,
+        .resume         = rt2x00soc_resume,
+};
+
+module_platform_driver(rt2800soc_driver);
+
+MODULE_AUTHOR(DRV_PROJECT);
+MODULE_VERSION(DRV_VERSION);
+MODULE_DESCRIPTION("Ralink WiSoC Wireless LAN driver.");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index 96677ce55da4..997df03a0c2e 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -148,6 +148,8 @@ static bool rt2800usb_txstatus_timeout(struct rt2x00_dev *rt2x00dev)
         return false;
 }
 
+#define TXSTATUS_READ_INTERVAL 1000000
+
 static bool rt2800usb_tx_sta_fifo_read_completed(struct rt2x00_dev *rt2x00dev,
                                                  int urb_status, u32 tx_status)
 {
@@ -176,8 +178,9 @@ static bool rt2800usb_tx_sta_fifo_read_completed(struct rt2x00_dev *rt2x00dev,
                 queue_work(rt2x00dev->workqueue, &rt2x00dev->txdone_work);
 
         if (rt2800usb_txstatus_pending(rt2x00dev)) {
-                /* Read register after 250 us */
-                hrtimer_start(&rt2x00dev->txstatus_timer, ktime_set(0, 250000),
+                /* Read register after 1 ms */
+                hrtimer_start(&rt2x00dev->txstatus_timer,
+                              ktime_set(0, TXSTATUS_READ_INTERVAL),
                               HRTIMER_MODE_REL);
                 return false;
         }
@@ -202,8 +205,9 @@ static void rt2800usb_async_read_tx_status(struct rt2x00_dev *rt2x00dev)
         if (test_and_set_bit(TX_STATUS_READING, &rt2x00dev->flags))
                 return;
 
-        /* Read TX_STA_FIFO register after 500 us */
-        hrtimer_start(&rt2x00dev->txstatus_timer, ktime_set(0, 500000),
+        /* Read TX_STA_FIFO register after 2 ms */
+        hrtimer_start(&rt2x00dev->txstatus_timer,
+                      ktime_set(0, 2*TXSTATUS_READ_INTERVAL),
                       HRTIMER_MODE_REL);
 }
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index 51f17cfb93f9..7c157857f5ce 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -754,6 +754,9 @@ void rt2x00mac_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
         struct rt2x00_dev *rt2x00dev = hw->priv;
         struct data_queue *queue;
 
+        if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
+                return;
+
         tx_queue_for_each(rt2x00dev, queue)
                 rt2x00queue_flush_queue(queue, drop);
 }
diff --git a/drivers/nfc/Kconfig b/drivers/nfc/Kconfig
index b0b64ccb7d7d..c1fb20603338 100644
--- a/drivers/nfc/Kconfig
+++ b/drivers/nfc/Kconfig
@@ -46,6 +46,16 @@ config NFC_SIM
 
           If unsure, say N.
 
+config NFC_PORT100
+        tristate "Sony NFC Port-100 Series USB device support"
+        depends on USB
+        depends on NFC_DIGITAL
+        help
+          This adds support for Sony Port-100 chip based USB devices such as the
+          RC-S380 dongle.
+
+          If unsure, say N.
+
 source "drivers/nfc/pn544/Kconfig"
 source "drivers/nfc/microread/Kconfig"
 
diff --git a/drivers/nfc/Makefile b/drivers/nfc/Makefile
index be7636abcb3f..c715fe8582a8 100644
--- a/drivers/nfc/Makefile
+++ b/drivers/nfc/Makefile
@@ -8,5 +8,6 @@ obj-$(CONFIG_NFC_PN533)		+= pn533.o
 obj-$(CONFIG_NFC_WILINK)        += nfcwilink.o
 obj-$(CONFIG_NFC_MEI_PHY)       += mei_phy.o
 obj-$(CONFIG_NFC_SIM)           += nfcsim.o
+obj-$(CONFIG_NFC_PORT100)       += port100.o
 
 ccflags-$(CONFIG_NFC_DEBUG) := -DDEBUG
diff --git a/drivers/nfc/mei_phy.c b/drivers/nfc/mei_phy.c
index 606bf55e76ec..85f90090cc1d 100644
--- a/drivers/nfc/mei_phy.c
+++ b/drivers/nfc/mei_phy.c
@@ -18,6 +18,8 @@
  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/nfc.h>
@@ -60,13 +62,13 @@ int nfc_mei_phy_enable(void *phy_id)
 
         r = mei_cl_enable_device(phy->device);
         if (r < 0) {
-                pr_err("MEI_PHY: Could not enable device\n");
+                pr_err("Could not enable device\n");
                 return r;
         }
 
         r = mei_cl_register_event_cb(phy->device, nfc_mei_event_cb, phy);
         if (r) {
-                pr_err("MEY_PHY: Event cb registration failed\n");
+                pr_err("Event cb registration failed\n");
                 mei_cl_disable_device(phy->device);
                 phy->powered = 0;
 
diff --git a/drivers/nfc/microread/i2c.c b/drivers/nfc/microread/i2c.c
index 101089495bf8..696e3467eccc 100644
--- a/drivers/nfc/microread/i2c.c
+++ b/drivers/nfc/microread/i2c.c
@@ -18,6 +18,8 @@
  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/delay.h>
@@ -95,12 +97,8 @@ static int check_crc(struct sk_buff *skb)
                 crc = crc ^ skb->data[i];
 
         if (crc != skb->data[skb->len-1]) {
-                pr_err(MICROREAD_I2C_DRIVER_NAME
-                       ": CRC error 0x%x != 0x%x\n",
-                       crc, skb->data[skb->len-1]);
-
-                pr_info(DRIVER_DESC ": %s : BAD CRC\n", __func__);
-
+                pr_err("CRC error 0x%x != 0x%x\n", crc, skb->data[skb->len-1]);
+                pr_info("%s: BAD CRC\n", __func__);
                 return -EPERM;
         }
 
@@ -160,18 +158,15 @@ static int microread_i2c_read(struct microread_i2c_phy *phy,
         u8 tmp[MICROREAD_I2C_LLC_MAX_SIZE - 1];
         struct i2c_client *client = phy->i2c_dev;
 
-        pr_debug("%s\n", __func__);
-
         r = i2c_master_recv(client, &len, 1);
         if (r != 1) {
-                dev_err(&client->dev, "cannot read len byte\n");
+                nfc_err(&client->dev, "cannot read len byte\n");
                 return -EREMOTEIO;
         }
 
         if ((len < MICROREAD_I2C_LLC_MIN_SIZE) ||
             (len > MICROREAD_I2C_LLC_MAX_SIZE)) {
-                dev_err(&client->dev, "invalid len byte\n");
-                pr_err("invalid len byte\n");
+                nfc_err(&client->dev, "invalid len byte\n");
                 r = -EBADMSG;
                 goto flush;
         }
@@ -228,7 +223,6 @@ static irqreturn_t microread_i2c_irq_thread_fn(int irq, void *phy_id)
         }
 
         client = phy->i2c_dev;
-        dev_dbg(&client->dev, "IRQ\n");
 
         if (phy->hard_fault != 0)
                 return IRQ_HANDLED;
@@ -263,20 +257,18 @@ static int microread_i2c_probe(struct i2c_client *client,
                 dev_get_platdata(&client->dev);
         int r;
 
-        dev_dbg(&client->dev, "client %p", client);
+        dev_dbg(&client->dev, "client %p\n", client);
 
         if (!pdata) {
-                dev_err(&client->dev, "client %p: missing platform data",
+                nfc_err(&client->dev, "client %p: missing platform data\n",
                         client);
                 return -EINVAL;
         }
 
         phy = devm_kzalloc(&client->dev, sizeof(struct microread_i2c_phy),
                            GFP_KERNEL);
-        if (!phy) {
-                dev_err(&client->dev, "Can't allocate microread phy");
+        if (!phy)
                 return -ENOMEM;
-        }
 
         i2c_set_clientdata(client, phy);
         phy->i2c_dev = client;
@@ -285,7 +277,7 @@ static int microread_i2c_probe(struct i2c_client *client,
                                  IRQF_TRIGGER_RISING | IRQF_ONESHOT,
                                  MICROREAD_I2C_DRIVER_NAME, phy);
         if (r) {
-                dev_err(&client->dev, "Unable to register IRQ handler");
+                nfc_err(&client->dev, "Unable to register IRQ handler\n");
                 return r;
         }
 
@@ -296,7 +288,7 @@ static int microread_i2c_probe(struct i2c_client *client,
         if (r < 0)
                 goto err_irq;
 
-        dev_info(&client->dev, "Probed");
+        nfc_info(&client->dev, "Probed");
 
         return 0;
 
@@ -310,8 +302,6 @@ static int microread_i2c_remove(struct i2c_client *client)
 {
         struct microread_i2c_phy *phy = i2c_get_clientdata(client);
 
-        dev_dbg(&client->dev, "%s\n", __func__);
-
         microread_remove(phy->hdev);
 
         free_irq(client->irq, phy);
diff --git a/drivers/nfc/microread/mei.c b/drivers/nfc/microread/mei.c
index cdf1bc53b257..72fafec3d460 100644
--- a/drivers/nfc/microread/mei.c
+++ b/drivers/nfc/microread/mei.c
@@ -18,6 +18,8 @@
  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/mod_devicetable.h>
 #include <linux/nfc.h>
@@ -59,8 +61,6 @@ static int microread_mei_remove(struct mei_cl_device *device)
 {
         struct nfc_mei_phy *phy = mei_cl_get_drvdata(device);
 
-        pr_info("Removing microread\n");
-
         microread_remove(phy->hdev);
 
         nfc_mei_phy_free(phy);
diff --git a/drivers/nfc/microread/microread.c b/drivers/nfc/microread/microread.c
index cdb9f6de132a..970ded6bfcf5 100644
--- a/drivers/nfc/microread/microread.c
+++ b/drivers/nfc/microread/microread.c
@@ -18,6 +18,8 @@
  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/module.h>
 #include <linux/delay.h>
 #include <linux/slab.h>
@@ -546,7 +548,7 @@ exit:
         kfree_skb(skb);
 
         if (r)
-                pr_err("Failed to handle discovered target err=%d", r);
+                pr_err("Failed to handle discovered target err=%d\n", r);
 }
 
 static int microread_event_received(struct nfc_hci_dev *hdev, u8 gate,
@@ -656,7 +658,6 @@ int microread_probe(void *phy_id, struct nfc_phy_ops *phy_ops, char *llc_name,
 
         info = kzalloc(sizeof(struct microread_info), GFP_KERNEL);
         if (!info) {
-                pr_err("Cannot allocate memory for microread_info.\n");
                 r = -ENOMEM;
                 goto err_info_alloc;
         }
@@ -686,7 +687,7 @@ int microread_probe(void *phy_id, struct nfc_phy_ops *phy_ops, char *llc_name,
                                              MICROREAD_CMD_TAILROOM,
                                              phy_payload);
         if (!info->hdev) {
-                pr_err("Cannot allocate nfc hdev.\n");
+                pr_err("Cannot allocate nfc hdev\n");
                 r = -ENOMEM;
                 goto err_alloc_hdev;
         }
diff --git a/drivers/nfc/nfcsim.c b/drivers/nfc/nfcsim.c
index 9a53f13c88df..93111fa8d282 100644
--- a/drivers/nfc/nfcsim.c
+++ b/drivers/nfc/nfcsim.c
@@ -19,10 +19,10 @@
 #include <linux/nfc.h>
 #include <net/nfc/nfc.h>
 
-#define DEV_ERR(_dev, fmt, args...) nfc_dev_err(&_dev->nfc_dev->dev, \
+#define DEV_ERR(_dev, fmt, args...) nfc_err(&_dev->nfc_dev->dev, \
                                                 "%s: " fmt, __func__, ## args)
 
-#define DEV_DBG(_dev, fmt, args...) nfc_dev_dbg(&_dev->nfc_dev->dev, \
+#define DEV_DBG(_dev, fmt, args...) dev_dbg(&_dev->nfc_dev->dev, \
                                                 "%s: " fmt, __func__, ## args)
 
 #define NFCSIM_VERSION "0.1"
@@ -64,7 +64,7 @@ static struct workqueue_struct *wq;
 
 static void nfcsim_cleanup_dev(struct nfcsim *dev, u8 shutdown)
 {
-        DEV_DBG(dev, "shutdown=%d", shutdown);
+        DEV_DBG(dev, "shutdown=%d\n", shutdown);
 
         mutex_lock(&dev->lock);
 
@@ -84,7 +84,7 @@ static int nfcsim_target_found(struct nfcsim *dev)
 {
         struct nfc_target nfc_tgt;
 
-        DEV_DBG(dev, "");
+        DEV_DBG(dev, "\n");
 
         memset(&nfc_tgt, 0, sizeof(struct nfc_target));
 
@@ -98,7 +98,7 @@ static int nfcsim_dev_up(struct nfc_dev *nfc_dev)
 {
         struct nfcsim *dev = nfc_get_drvdata(nfc_dev);
 
-        DEV_DBG(dev, "");
+        DEV_DBG(dev, "\n");
 
         mutex_lock(&dev->lock);
 
@@ -113,7 +113,7 @@ static int nfcsim_dev_down(struct nfc_dev *nfc_dev)
 {
         struct nfcsim *dev = nfc_get_drvdata(nfc_dev);
 
-        DEV_DBG(dev, "");
+        DEV_DBG(dev, "\n");
 
         mutex_lock(&dev->lock);
 
@@ -143,7 +143,7 @@ static int nfcsim_dep_link_up(struct nfc_dev *nfc_dev,
 
         remote_gb = nfc_get_local_general_bytes(peer->nfc_dev, &remote_gb_len);
         if (!remote_gb) {
-                DEV_ERR(peer, "Can't get remote general bytes");
+                DEV_ERR(peer, "Can't get remote general bytes\n");
 
                 mutex_unlock(&peer->lock);
                 return -EINVAL;
@@ -155,7 +155,7 @@ static int nfcsim_dep_link_up(struct nfc_dev *nfc_dev,
 
         rc = nfc_set_remote_general_bytes(nfc_dev, remote_gb, remote_gb_len);
         if (rc) {
-                DEV_ERR(dev, "Can't set remote general bytes");
+                DEV_ERR(dev, "Can't set remote general bytes\n");
                 mutex_unlock(&dev->lock);
                 return rc;
         }
@@ -172,7 +172,7 @@ static int nfcsim_dep_link_down(struct nfc_dev *nfc_dev)
 {
         struct nfcsim *dev = nfc_get_drvdata(nfc_dev);
 
-        DEV_DBG(dev, "");
+        DEV_DBG(dev, "\n");
 
         nfcsim_cleanup_dev(dev, 0);
 
@@ -188,7 +188,7 @@ static int nfcsim_start_poll(struct nfc_dev *nfc_dev,
         mutex_lock(&dev->lock);
 
         if (dev->polling_mode != NFCSIM_POLL_NONE) {
-                DEV_ERR(dev, "Already in polling mode");
+                DEV_ERR(dev, "Already in polling mode\n");
                 rc = -EBUSY;
                 goto exit;
         }
@@ -200,7 +200,7 @@ static int nfcsim_start_poll(struct nfc_dev *nfc_dev,
                 dev->polling_mode |= NFCSIM_POLL_TARGET;
 
         if (dev->polling_mode == NFCSIM_POLL_NONE) {
-                DEV_ERR(dev, "Unsupported polling mode");
+                DEV_ERR(dev, "Unsupported polling mode\n");
                 rc = -EINVAL;
                 goto exit;
         }
@@ -210,7 +210,7 @@ static int nfcsim_start_poll(struct nfc_dev *nfc_dev,
 
         queue_delayed_work(wq, &dev->poll_work, 0);
 
-        DEV_DBG(dev, "Start polling: im: 0x%X, tm: 0x%X", im_protocols,
+        DEV_DBG(dev, "Start polling: im: 0x%X, tm: 0x%X\n", im_protocols,
                 tm_protocols);
 
         rc = 0;
@@ -224,7 +224,7 @@ static void nfcsim_stop_poll(struct nfc_dev *nfc_dev)
 {
         struct nfcsim *dev = nfc_get_drvdata(nfc_dev);
 
-        DEV_DBG(dev, "Stop poll");
+        DEV_DBG(dev, "Stop poll\n");
 
         mutex_lock(&dev->lock);
 
@@ -240,7 +240,7 @@ static int nfcsim_activate_target(struct nfc_dev *nfc_dev,
 {
         struct nfcsim *dev = nfc_get_drvdata(nfc_dev);
 
-        DEV_DBG(dev, "");
+        DEV_DBG(dev, "\n");
 
         return -ENOTSUPP;
 }
@@ -250,7 +250,7 @@ static void nfcsim_deactivate_target(struct nfc_dev *nfc_dev,
 {
         struct nfcsim *dev = nfc_get_drvdata(nfc_dev);
 
-        DEV_DBG(dev, "");
+        DEV_DBG(dev, "\n");
 }
 
 static void nfcsim_wq_recv(struct work_struct *work)
@@ -267,7 +267,7 @@ static void nfcsim_wq_recv(struct work_struct *work)
 
         if (dev->initiator) {
                 if (!dev->cb) {
-                        DEV_ERR(dev, "Null recv callback");
+                        DEV_ERR(dev, "Null recv callback\n");
                         dev_kfree_skb(dev->clone_skb);
                         goto exit;
                 }
@@ -310,7 +310,7 @@ static int nfcsim_tx(struct nfc_dev *nfc_dev, struct nfc_target *target,
         peer->clone_skb = skb_clone(skb, GFP_KERNEL);
 
         if (!peer->clone_skb) {
-                DEV_ERR(dev, "skb_clone failed");
+                DEV_ERR(dev, "skb_clone failed\n");
                 mutex_unlock(&peer->lock);
                 err = -ENOMEM;
                 goto exit;
@@ -397,13 +397,13 @@ static void nfcsim_wq_poll(struct work_struct *work)
         nfcsim_set_polling_mode(dev);
 
         if (dev->curr_polling_mode == NFCSIM_POLL_NONE) {
-                DEV_DBG(dev, "Not polling");
+                DEV_DBG(dev, "Not polling\n");
                 goto unlock;
         }
 
         DEV_DBG(dev, "Polling as %s",
                 dev->curr_polling_mode == NFCSIM_POLL_INITIATOR ?
-                "initiator" : "target");
+                "initiator\n" : "target\n");
 
         if (dev->curr_polling_mode == NFCSIM_POLL_TARGET)
                 goto sched_work;
diff --git a/drivers/nfc/nfcwilink.c b/drivers/nfc/nfcwilink.c
index 59f95d8fc98c..71308645593f 100644
--- a/drivers/nfc/nfcwilink.c
+++ b/drivers/nfc/nfcwilink.c
@@ -146,13 +146,11 @@ static int nfcwilink_get_bts_file_name(struct nfcwilink *drv, char *file_name)
         unsigned long comp_ret;
         int rc;
 
-        nfc_dev_dbg(&drv->pdev->dev, "get_bts_file_name entry");
-
         skb = nfcwilink_skb_alloc(sizeof(struct nci_vs_nfcc_info_cmd),
                                         GFP_KERNEL);
         if (!skb) {
-                nfc_dev_err(&drv->pdev->dev,
-                                "no memory for nci_vs_nfcc_info_cmd");
+                nfc_err(&drv->pdev->dev,
+                        "no memory for nci_vs_nfcc_info_cmd\n");
                 return -ENOMEM;
         }
 
@@ -170,21 +168,19 @@ static int nfcwilink_get_bts_file_name(struct nfcwilink *drv, char *file_name)
 
         comp_ret = wait_for_completion_timeout(&drv->completed,
                                 msecs_to_jiffies(NFCWILINK_CMD_TIMEOUT));
-        nfc_dev_dbg(&drv->pdev->dev, "wait_for_completion_timeout returned %ld",
-                        comp_ret);
+        dev_dbg(&drv->pdev->dev, "wait_for_completion_timeout returned %ld\n",
+                comp_ret);
         if (comp_ret == 0) {
-                nfc_dev_err(&drv->pdev->dev,
-                                "timeout on wait_for_completion_timeout");
+                nfc_err(&drv->pdev->dev,
+                        "timeout on wait_for_completion_timeout\n");
                 return -ETIMEDOUT;
         }
 
-        nfc_dev_dbg(&drv->pdev->dev, "nci_vs_nfcc_info_rsp: plen %d, status %d",
-                        drv->nfcc_info.plen,
-                        drv->nfcc_info.status);
+        dev_dbg(&drv->pdev->dev, "nci_vs_nfcc_info_rsp: plen %d, status %d\n",
+                drv->nfcc_info.plen, drv->nfcc_info.status);
 
         if ((drv->nfcc_info.plen != 5) || (drv->nfcc_info.status != 0)) {
-                nfc_dev_err(&drv->pdev->dev,
-                                "invalid nci_vs_nfcc_info_rsp");
+                nfc_err(&drv->pdev->dev, "invalid nci_vs_nfcc_info_rsp\n");
                 return -EINVAL;
         }
 
@@ -195,7 +191,7 @@ static int nfcwilink_get_bts_file_name(struct nfcwilink *drv, char *file_name)
                         drv->nfcc_info.sw_ver_z,
                         drv->nfcc_info.patch_id);
 
-        nfc_dev_info(&drv->pdev->dev, "nfcwilink FW file name: %s", file_name);
+        nfc_info(&drv->pdev->dev, "nfcwilink FW file name: %s\n", file_name);
 
         return 0;
 }
@@ -207,15 +203,13 @@ static int nfcwilink_send_bts_cmd(struct nfcwilink *drv, __u8 *data, int len)
         unsigned long comp_ret;
         int rc;
 
-        nfc_dev_dbg(&drv->pdev->dev, "send_bts_cmd entry");
-
         /* verify valid cmd for the NFC channel */
         if ((len <= sizeof(struct nfcwilink_hdr)) ||
                 (len > BTS_FILE_CMD_MAX_LEN) ||
                 (hdr->chnl != NFCWILINK_CHNL) ||
                 (hdr->opcode != NFCWILINK_OPCODE)) {
-                nfc_dev_err(&drv->pdev->dev,
-                        "ignoring invalid bts cmd, len %d, chnl %d, opcode %d",
+                nfc_err(&drv->pdev->dev,
+                        "ignoring invalid bts cmd, len %d, chnl %d, opcode %d\n",
                         len, hdr->chnl, hdr->opcode);
                 return 0;
         }
@@ -226,7 +220,7 @@ static int nfcwilink_send_bts_cmd(struct nfcwilink *drv, __u8 *data, int len)
 
         skb = nfcwilink_skb_alloc(len, GFP_KERNEL);
         if (!skb) {
-                nfc_dev_err(&drv->pdev->dev, "no memory for bts cmd");
+                nfc_err(&drv->pdev->dev, "no memory for bts cmd\n");
                 return -ENOMEM;
         }
 
@@ -238,11 +232,11 @@ static int nfcwilink_send_bts_cmd(struct nfcwilink *drv, __u8 *data, int len)
 
         comp_ret = wait_for_completion_timeout(&drv->completed,
                                 msecs_to_jiffies(NFCWILINK_CMD_TIMEOUT));
-        nfc_dev_dbg(&drv->pdev->dev, "wait_for_completion_timeout returned %ld",
-                        comp_ret);
+        dev_dbg(&drv->pdev->dev, "wait_for_completion_timeout returned %ld\n",
+                comp_ret);
         if (comp_ret == 0) {
-                nfc_dev_err(&drv->pdev->dev,
-                                "timeout on wait_for_completion_timeout");
+                nfc_err(&drv->pdev->dev,
+                        "timeout on wait_for_completion_timeout\n");
                 return -ETIMEDOUT;
         }
 
@@ -257,8 +251,6 @@ static int nfcwilink_download_fw(struct nfcwilink *drv)
         __u8 *ptr;
         int len, rc;
 
-        nfc_dev_dbg(&drv->pdev->dev, "download_fw entry");
-
         set_bit(NFCWILINK_FW_DOWNLOAD, &drv->flags);
 
         rc = nfcwilink_get_bts_file_name(drv, file_name);
@@ -267,7 +259,7 @@ static int nfcwilink_download_fw(struct nfcwilink *drv)
 
         rc = request_firmware(&fw, file_name, &drv->pdev->dev);
         if (rc) {
-                nfc_dev_err(&drv->pdev->dev, "request_firmware failed %d", rc);
+                nfc_err(&drv->pdev->dev, "request_firmware failed %d\n", rc);
 
                 /* if the file is not found, don't exit with failure */
                 if (rc == -ENOENT)
@@ -280,14 +272,14 @@ static int nfcwilink_download_fw(struct nfcwilink *drv)
         ptr = (__u8 *)fw->data;
 
         if ((len == 0) || (ptr == NULL)) {
-                nfc_dev_dbg(&drv->pdev->dev,
-                                "request_firmware returned size %d", len);
+                dev_dbg(&drv->pdev->dev,
+                        "request_firmware returned size %d\n", len);
                 goto release_fw;
         }
 
         if (__le32_to_cpu(((struct bts_file_hdr *)ptr)->magic) !=
                         BTS_FILE_HDR_MAGIC) {
-                nfc_dev_err(&drv->pdev->dev, "wrong bts magic number");
+                nfc_err(&drv->pdev->dev, "wrong bts magic number\n");
                 rc = -EINVAL;
                 goto release_fw;
         }
@@ -302,8 +294,8 @@ static int nfcwilink_download_fw(struct nfcwilink *drv)
                 action_len =
                         __le16_to_cpu(((struct bts_file_action *)ptr)->len);
 
-                nfc_dev_dbg(&drv->pdev->dev, "bts_file_action type %d, len %d",
-                                action_type, action_len);
+                dev_dbg(&drv->pdev->dev, "bts_file_action type %d, len %d\n",
+                        action_type, action_len);
 
                 switch (action_type) {
                 case BTS_FILE_ACTION_TYPE_SEND_CMD:
@@ -333,8 +325,6 @@ static void nfcwilink_register_complete(void *priv_data, char data)
 {
         struct nfcwilink *drv = priv_data;
 
-        nfc_dev_dbg(&drv->pdev->dev, "register_complete entry");
-
         /* store ST registration status */
         drv->st_register_cb_status = data;
 
@@ -356,7 +346,7 @@ static long nfcwilink_receive(void *priv_data, struct sk_buff *skb)
                 return -EFAULT;
         }
 
-        nfc_dev_dbg(&drv->pdev->dev, "receive entry, len %d", skb->len);
+        dev_dbg(&drv->pdev->dev, "receive entry, len %d\n", skb->len);
 
         /* strip the ST header
         (apart for the chnl byte, which is not received in the hdr) */
@@ -370,7 +360,7 @@ static long nfcwilink_receive(void *priv_data, struct sk_buff *skb)
         /* Forward skb to NCI core layer */
         rc = nci_recv_frame(drv->ndev, skb);
         if (rc < 0) {
-                nfc_dev_err(&drv->pdev->dev, "nci_recv_frame failed %d", rc);
+                nfc_err(&drv->pdev->dev, "nci_recv_frame failed %d\n", rc);
                 return rc;
         }
 
@@ -396,8 +386,6 @@ static int nfcwilink_open(struct nci_dev *ndev)
         unsigned long comp_ret;
         int rc;
 
-        nfc_dev_dbg(&drv->pdev->dev, "open entry");
-
         if (test_and_set_bit(NFCWILINK_RUNNING, &drv->flags)) {
                 rc = -EBUSY;
                 goto exit;
@@ -415,9 +403,9 @@ static int nfcwilink_open(struct nci_dev *ndev)
                         &drv->completed,
                         msecs_to_jiffies(NFCWILINK_REGISTER_TIMEOUT));
 
-                        nfc_dev_dbg(&drv->pdev->dev,
-                        "wait_for_completion_timeout returned %ld",
-                        comp_ret);
+                        dev_dbg(&drv->pdev->dev,
+                                "wait_for_completion_timeout returned %ld\n",
+                                comp_ret);
 
                         if (comp_ret == 0) {
                                 /* timeout */
@@ -425,13 +413,12 @@ static int nfcwilink_open(struct nci_dev *ndev)
                                 goto clear_exit;
                         } else if (drv->st_register_cb_status != 0) {
                                 rc = drv->st_register_cb_status;
-                                nfc_dev_err(&drv->pdev->dev,
-                                "st_register_cb failed %d", rc);
+                                nfc_err(&drv->pdev->dev,
+                                        "st_register_cb failed %d\n", rc);
                                 goto clear_exit;
                         }
                 } else {
-                        nfc_dev_err(&drv->pdev->dev,
-                                "st_register failed %d", rc);
+                        nfc_err(&drv->pdev->dev, "st_register failed %d\n", rc);
                         goto clear_exit;
                 }
         }
@@ -441,8 +428,8 @@ static int nfcwilink_open(struct nci_dev *ndev)
         drv->st_write = nfcwilink_proto.write;
 
         if (nfcwilink_download_fw(drv)) {
-                nfc_dev_err(&drv->pdev->dev, "nfcwilink_download_fw failed %d",
-                                rc);
+                nfc_err(&drv->pdev->dev, "nfcwilink_download_fw failed %d\n",
+                        rc);
                 /* open should succeed, even if the FW download failed */
         }
 
@@ -460,14 +447,12 @@ static int nfcwilink_close(struct nci_dev *ndev)
         struct nfcwilink *drv = nci_get_drvdata(ndev);
         int rc;
 
-        nfc_dev_dbg(&drv->pdev->dev, "close entry");
-
         if (!test_and_clear_bit(NFCWILINK_RUNNING, &drv->flags))
                 return 0;
 
         rc = st_unregister(&nfcwilink_proto);
         if (rc)
-                nfc_dev_err(&drv->pdev->dev, "st_unregister failed %d", rc);
+                nfc_err(&drv->pdev->dev, "st_unregister failed %d\n", rc);
 
         drv->st_write = NULL;
 
@@ -480,7 +465,7 @@ static int nfcwilink_send(struct nci_dev *ndev, struct sk_buff *skb)
         struct nfcwilink_hdr hdr = {NFCWILINK_CHNL, NFCWILINK_OPCODE, 0x0000};
         long len;
 
-        nfc_dev_dbg(&drv->pdev->dev, "send entry, len %d", skb->len);
+        dev_dbg(&drv->pdev->dev, "send entry, len %d\n", skb->len);
 
         if (!test_bit(NFCWILINK_RUNNING, &drv->flags)) {
                 kfree_skb(skb);
@@ -498,7 +483,7 @@ static int nfcwilink_send(struct nci_dev *ndev, struct sk_buff *skb)
         len = drv->st_write(skb);
         if (len < 0) {
                 kfree_skb(skb);
-                nfc_dev_err(&drv->pdev->dev, "st_write failed %ld", len);
+                nfc_err(&drv->pdev->dev, "st_write failed %ld\n", len);
                 return -EFAULT;
         }
 
@@ -517,8 +502,6 @@ static int nfcwilink_probe(struct platform_device *pdev)
         int rc;
         __u32 protocols;
 
-        nfc_dev_dbg(&pdev->dev, "probe entry");
-
         drv = devm_kzalloc(&pdev->dev, sizeof(struct nfcwilink), GFP_KERNEL);
         if (!drv) {
                 rc = -ENOMEM;
@@ -538,7 +521,7 @@ static int nfcwilink_probe(struct platform_device *pdev)
                                         NFCWILINK_HDR_LEN,
                                         0);
         if (!drv->ndev) {
-                nfc_dev_err(&pdev->dev, "nci_allocate_device failed");
+                nfc_err(&pdev->dev, "nci_allocate_device failed\n");
                 rc = -ENOMEM;
                 goto exit;
         }
@@ -548,7 +531,7 @@ static int nfcwilink_probe(struct platform_device *pdev)
 
         rc = nci_register_device(drv->ndev);
         if (rc < 0) {
-                nfc_dev_err(&pdev->dev, "nci_register_device failed %d", rc);
+                nfc_err(&pdev->dev, "nci_register_device failed %d\n", rc);
                 goto free_dev_exit;
         }
 
@@ -568,8 +551,6 @@ static int nfcwilink_remove(struct platform_device *pdev)
         struct nfcwilink *drv = dev_get_drvdata(&pdev->dev);
         struct nci_dev *ndev;
 
-        nfc_dev_dbg(&pdev->dev, "remove entry");
-
         if (!drv)
                 return -EFAULT;
 
@@ -578,8 +559,6 @@ static int nfcwilink_remove(struct platform_device *pdev)
         nci_unregister_device(ndev);
         nci_free_device(ndev);
 
-        dev_set_drvdata(&pdev->dev, NULL);
-
         return 0;
 }
 
diff --git a/drivers/nfc/pn533.c b/drivers/nfc/pn533.c
index 5df730be88a3..2daf04c07338 100644
--- a/drivers/nfc/pn533.c
+++ b/drivers/nfc/pn533.c
@@ -150,6 +150,7 @@ MODULE_DEVICE_TABLE(usb, pn533_table);
 #define PN533_CMD_TG_INIT_AS_TARGET 0x8c
 #define PN533_CMD_TG_GET_DATA 0x86
 #define PN533_CMD_TG_SET_DATA 0x8e
+#define PN533_CMD_TG_SET_META_DATA 0x94
 #define PN533_CMD_UNDEF 0xff
 
 #define PN533_CMD_RESPONSE(cmd) (cmd + 1)
@@ -373,6 +374,8 @@ struct pn533 {
         struct delayed_work poll_work;
         struct work_struct mi_rx_work;
         struct work_struct mi_tx_work;
+        struct work_struct mi_tm_rx_work;
+        struct work_struct mi_tm_tx_work;
         struct work_struct tg_work;
         struct work_struct rf_work;
 
@@ -387,6 +390,7 @@ struct pn533 {
         struct pn533_poll_modulations *poll_mod_active[PN533_POLL_MOD_MAX + 1];
         u8 poll_mod_count;
         u8 poll_mod_curr;
+        u8 poll_dep;
         u32 poll_protocols;
         u32 listen_protocols;
         struct timer_list listen_timer;
@@ -722,32 +726,32 @@ static void pn533_recv_response(struct urb *urb)
                 break; /* success */
         case -ECONNRESET:
         case -ENOENT:
-                nfc_dev_dbg(&dev->interface->dev,
-                            "The urb has been canceled (status %d)",
-                            urb->status);
+                dev_dbg(&dev->interface->dev,
+                        "The urb has been canceled (status %d)\n",
+                        urb->status);
                 goto sched_wq;
         case -ESHUTDOWN:
         default:
-                nfc_dev_err(&dev->interface->dev,
-                            "Urb failure (status %d)", urb->status);
+                nfc_err(&dev->interface->dev,
+                        "Urb failure (status %d)\n", urb->status);
                 goto sched_wq;
         }
 
         in_frame = dev->in_urb->transfer_buffer;
 
-        nfc_dev_dbg(&dev->interface->dev, "Received a frame.");
+        dev_dbg(&dev->interface->dev, "Received a frame\n");
         print_hex_dump_debug("PN533 RX: ", DUMP_PREFIX_NONE, 16, 1, in_frame,
                              dev->ops->rx_frame_size(in_frame), false);
 
         if (!dev->ops->rx_is_frame_valid(in_frame, dev)) {
-                nfc_dev_err(&dev->interface->dev, "Received an invalid frame");
+                nfc_err(&dev->interface->dev, "Received an invalid frame\n");
                 cmd->status = -EIO;
                 goto sched_wq;
         }
 
         if (!pn533_rx_frame_is_cmd_response(dev, in_frame)) {
-                nfc_dev_err(&dev->interface->dev,
-                            "It it not the response to the last command");
+                nfc_err(&dev->interface->dev,
+                        "It it not the response to the last command\n");
                 cmd->status = -EIO;
                 goto sched_wq;
         }
@@ -777,29 +781,29 @@ static void pn533_recv_ack(struct urb *urb)
                 break; /* success */
         case -ECONNRESET:
         case -ENOENT:
-                nfc_dev_dbg(&dev->interface->dev,
-                            "The urb has been stopped (status %d)",
-                            urb->status);
+                dev_dbg(&dev->interface->dev,
+                        "The urb has been stopped (status %d)\n",
+                        urb->status);
                 goto sched_wq;
         case -ESHUTDOWN:
         default:
-                nfc_dev_err(&dev->interface->dev,
-                            "Urb failure (status %d)", urb->status);
+                nfc_err(&dev->interface->dev,
+                        "Urb failure (status %d)\n", urb->status);
                 goto sched_wq;
         }
 
         in_frame = dev->in_urb->transfer_buffer;
 
         if (!pn533_std_rx_frame_is_ack(in_frame)) {
-                nfc_dev_err(&dev->interface->dev, "Received an invalid ack");
+                nfc_err(&dev->interface->dev, "Received an invalid ack\n");
                 cmd->status = -EIO;
                 goto sched_wq;
         }
 
         rc = pn533_submit_urb_for_response(dev, GFP_ATOMIC);
         if (rc) {
-                nfc_dev_err(&dev->interface->dev,
-                            "usb_submit_urb failed with result %d", rc);
+                nfc_err(&dev->interface->dev,
+                        "usb_submit_urb failed with result %d\n", rc);
                 cmd->status = rc;
                 goto sched_wq;
         }
@@ -823,8 +827,6 @@ static int pn533_send_ack(struct pn533 *dev, gfp_t flags)
         /* spec 7.1.1.3:  Preamble, SoPC (2), ACK Code (2), Postamble */
         int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
-
         dev->out_urb->transfer_buffer = ack;
         dev->out_urb->transfer_buffer_length = sizeof(ack);
         rc = usb_submit_urb(dev->out_urb, flags);
@@ -927,7 +929,7 @@ static int __pn533_send_async(struct pn533 *dev, u8 cmd_code,
         struct pn533_cmd *cmd;
         int rc = 0;
 
-        nfc_dev_dbg(&dev->interface->dev, "Sending command 0x%x", cmd_code);
+        dev_dbg(&dev->interface->dev, "Sending command 0x%x\n", cmd_code);
 
         cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
         if (!cmd)
@@ -954,8 +956,8 @@ static int __pn533_send_async(struct pn533 *dev, u8 cmd_code,
                 goto unlock;
         }
 
-        nfc_dev_dbg(&dev->interface->dev, "%s Queueing command 0x%x", __func__,
-                    cmd_code);
+        dev_dbg(&dev->interface->dev, "%s Queueing command 0x%x\n",
+                __func__, cmd_code);
 
         INIT_LIST_HEAD(&cmd->queue);
         list_add_tail(&cmd->queue, &dev->cmd_queue);
@@ -1168,14 +1170,14 @@ static void pn533_send_complete(struct urb *urb)
                 break; /* success */
         case -ECONNRESET:
         case -ENOENT:
-                nfc_dev_dbg(&dev->interface->dev,
-                            "The urb has been stopped (status %d)",
-                            urb->status);
+                dev_dbg(&dev->interface->dev,
+                        "The urb has been stopped (status %d)\n",
+                        urb->status);
                 break;
         case -ESHUTDOWN:
         default:
-                nfc_dev_err(&dev->interface->dev,
-                            "Urb failure (status %d)", urb->status);
+                nfc_err(&dev->interface->dev, "Urb failure (status %d)\n",
+                        urb->status);
         }
 }
 
@@ -1452,8 +1454,8 @@ static int pn533_target_found(struct pn533 *dev, u8 tg, u8 *tgdata,
         struct nfc_target nfc_tgt;
         int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s - modulation=%d", __func__,
-                    dev->poll_mod_curr);
+        dev_dbg(&dev->interface->dev, "%s: modulation=%d\n",
+                __func__, dev->poll_mod_curr);
 
         if (tg != 1)
                 return -EPROTO;
@@ -1475,8 +1477,8 @@ static int pn533_target_found(struct pn533 *dev, u8 tg, u8 *tgdata,
                 rc = pn533_target_found_type_b(&nfc_tgt, tgdata, tgdata_len);
                 break;
         default:
-                nfc_dev_err(&dev->interface->dev,
-                            "Unknown current poll modulation");
+                nfc_err(&dev->interface->dev,
+                        "Unknown current poll modulation\n");
                 return -EPROTO;
         }
 
@@ -1484,14 +1486,14 @@ static int pn533_target_found(struct pn533 *dev, u8 tg, u8 *tgdata,
                 return rc;
 
         if (!(nfc_tgt.supported_protocols & dev->poll_protocols)) {
-                nfc_dev_dbg(&dev->interface->dev,
-                            "The Tg found doesn't have the desired protocol");
+                dev_dbg(&dev->interface->dev,
+                        "The Tg found doesn't have the desired protocol\n");
                 return -EAGAIN;
         }
 
-        nfc_dev_dbg(&dev->interface->dev,
-                    "Target found - supported protocols: 0x%x",
-                    nfc_tgt.supported_protocols);
+        dev_dbg(&dev->interface->dev,
+                "Target found - supported protocols: 0x%x\n",
+                nfc_tgt.supported_protocols);
 
         dev->tgt_available_prots = nfc_tgt.supported_protocols;
 
@@ -1548,7 +1550,8 @@ static int pn533_start_poll_complete(struct pn533 *dev, struct sk_buff *resp)
         u8 nbtg, tg, *tgdata;
         int rc, tgdata_len;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        /* Toggle the DEP polling */
+        dev->poll_dep = 1;
 
         nbtg = resp->data[0];
         tg = resp->data[1];
@@ -1624,37 +1627,130 @@ static struct sk_buff *pn533_alloc_poll_tg_frame(struct pn533 *dev)
 
 #define PN533_CMD_DATAEXCH_HEAD_LEN 1
 #define PN533_CMD_DATAEXCH_DATA_MAXLEN 262
+static void pn533_wq_tm_mi_recv(struct work_struct *work);
+static struct sk_buff *pn533_build_response(struct pn533 *dev);
+
 static int pn533_tm_get_data_complete(struct pn533 *dev, void *arg,
                                       struct sk_buff *resp)
 {
-        u8 status;
+        struct sk_buff *skb;
+        u8 status, ret, mi;
+        int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
-        if (IS_ERR(resp))
+        if (IS_ERR(resp)) {
+                skb_queue_purge(&dev->resp_q);
                 return PTR_ERR(resp);
+        }
 
         status = resp->data[0];
+
+        ret = status & PN533_CMD_RET_MASK;
+        mi = status & PN533_CMD_MI_MASK;
+
         skb_pull(resp, sizeof(status));
 
-        if (status != 0) {
-                nfc_tm_deactivated(dev->nfc_dev);
-                dev->tgt_mode = 0;
-                dev_kfree_skb(resp);
-                return 0;
+        if (ret != PN533_CMD_RET_SUCCESS) {
+                rc = -EIO;
+                goto error;
         }
 
-        return nfc_tm_data_received(dev->nfc_dev, resp);
+        skb_queue_tail(&dev->resp_q, resp);
+
+        if (mi) {
+                queue_work(dev->wq, &dev->mi_tm_rx_work);
+                return -EINPROGRESS;
+        }
+
+        skb = pn533_build_response(dev);
+        if (!skb) {
+                rc = -EIO;
+                goto error;
+        }
+
+        return nfc_tm_data_received(dev->nfc_dev, skb);
+
+error:
+        nfc_tm_deactivated(dev->nfc_dev);
+        dev->tgt_mode = 0;
+        skb_queue_purge(&dev->resp_q);
+        dev_kfree_skb(resp);
+
+        return rc;
+}
+
+static void pn533_wq_tm_mi_recv(struct work_struct *work)
+{
+        struct pn533 *dev = container_of(work, struct pn533, mi_tm_rx_work);
+        struct sk_buff *skb;
+        int rc;
+
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
+
+        skb = pn533_alloc_skb(dev, 0);
+        if (!skb)
+                return;
+
+        rc = pn533_send_cmd_direct_async(dev,
+                                        PN533_CMD_TG_GET_DATA,
+                                        skb,
+                                        pn533_tm_get_data_complete,
+                                        NULL);
+
+        if (rc < 0)
+                dev_kfree_skb(skb);
+
+        return;
+}
+
+static int pn533_tm_send_complete(struct pn533 *dev, void *arg,
+                                  struct sk_buff *resp);
+static void pn533_wq_tm_mi_send(struct work_struct *work)
+{
+        struct pn533 *dev = container_of(work, struct pn533, mi_tm_tx_work);
+        struct sk_buff *skb;
+        int rc;
+
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
+
+        /* Grab the first skb in the queue */
+        skb = skb_dequeue(&dev->fragment_skb);
+        if (skb == NULL) {      /* No more data */
+                /* Reset the queue for future use */
+                skb_queue_head_init(&dev->fragment_skb);
+                goto error;
+        }
+
+        /* last entry - remove MI bit */
+        if (skb_queue_len(&dev->fragment_skb) == 0) {
+                rc = pn533_send_cmd_direct_async(dev, PN533_CMD_TG_SET_DATA,
+                                        skb, pn533_tm_send_complete, NULL);
+        } else
+                rc = pn533_send_cmd_direct_async(dev,
+                                        PN533_CMD_TG_SET_META_DATA,
+                                        skb, pn533_tm_send_complete, NULL);
+
+        if (rc == 0) /* success */
+                return;
+
+        dev_err(&dev->interface->dev,
+                "Error %d when trying to perform set meta data_exchange", rc);
+
+        dev_kfree_skb(skb);
+
+error:
+        pn533_send_ack(dev, GFP_KERNEL);
+        queue_work(dev->wq, &dev->cmd_work);
 }
 
 static void pn533_wq_tg_get_data(struct work_struct *work)
 {
         struct pn533 *dev = container_of(work, struct pn533, tg_work);
-
         struct sk_buff *skb;
         int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         skb = pn533_alloc_skb(dev, 0);
         if (!skb)
@@ -1676,7 +1772,7 @@ static int pn533_init_target_complete(struct pn533 *dev, struct sk_buff *resp)
         size_t gb_len;
         int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         if (resp->len < ATR_REQ_GB_OFFSET + 1)
                 return -EINVAL;
@@ -1684,8 +1780,8 @@ static int pn533_init_target_complete(struct pn533 *dev, struct sk_buff *resp)
         mode = resp->data[0];
         cmd = &resp->data[1];
 
-        nfc_dev_dbg(&dev->interface->dev, "Target mode 0x%x len %d\n",
-                    mode, resp->len);
+        dev_dbg(&dev->interface->dev, "Target mode 0x%x len %d\n",
+                mode, resp->len);
 
         if ((mode & PN533_INIT_TARGET_RESP_FRAME_MASK) ==
             PN533_INIT_TARGET_RESP_ACTIVE)
@@ -1700,8 +1796,8 @@ static int pn533_init_target_complete(struct pn533 *dev, struct sk_buff *resp)
         rc = nfc_tm_activated(dev->nfc_dev, NFC_PROTO_NFC_DEP_MASK,
                               comm_mode, gb, gb_len);
         if (rc < 0) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Error when signaling target activation");
+                nfc_err(&dev->interface->dev,
+                        "Error when signaling target activation\n");
                 return rc;
         }
 
@@ -1715,7 +1811,7 @@ static void pn533_listen_mode_timer(unsigned long data)
 {
         struct pn533 *dev = (struct pn533 *)data;
 
-        nfc_dev_dbg(&dev->interface->dev, "Listen mode timeout");
+        dev_dbg(&dev->interface->dev, "Listen mode timeout\n");
 
         dev->cancel_listen = 1;
 
@@ -1730,13 +1826,12 @@ static int pn533_rf_complete(struct pn533 *dev, void *arg,
 {
         int rc = 0;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         if (IS_ERR(resp)) {
                 rc = PTR_ERR(resp);
 
-                nfc_dev_err(&dev->interface->dev, "%s RF setting error %d",
-                            __func__, rc);
+                nfc_err(&dev->interface->dev, "RF setting error %d", rc);
 
                 return rc;
         }
@@ -1754,7 +1849,7 @@ static void pn533_wq_rf(struct work_struct *work)
         struct sk_buff *skb;
         int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         skb = pn533_alloc_skb(dev, 2);
         if (!skb)
@@ -1767,25 +1862,136 @@ static void pn533_wq_rf(struct work_struct *work)
                                   pn533_rf_complete, NULL);
         if (rc < 0) {
                 dev_kfree_skb(skb);
-                nfc_dev_err(&dev->interface->dev, "RF setting error %d", rc);
+                nfc_err(&dev->interface->dev, "RF setting error %d\n", rc);
         }
 
         return;
 }
 
+static int pn533_poll_dep_complete(struct pn533 *dev, void *arg,
+                                   struct sk_buff *resp)
+{
+        struct pn533_cmd_jump_dep_response *rsp;
+        struct nfc_target nfc_target;
+        u8 target_gt_len;
+        int rc;
+
+        if (IS_ERR(resp))
+                return PTR_ERR(resp);
+
+        rsp = (struct pn533_cmd_jump_dep_response *)resp->data;
+
+        rc = rsp->status & PN533_CMD_RET_MASK;
+        if (rc != PN533_CMD_RET_SUCCESS) {
+                /* Not target found, turn radio off */
+                queue_work(dev->wq, &dev->rf_work);
+
+                dev_kfree_skb(resp);
+                return 0;
+        }
+
+        dev_dbg(&dev->interface->dev, "Creating new target");
+
+        nfc_target.supported_protocols = NFC_PROTO_NFC_DEP_MASK;
+        nfc_target.nfcid1_len = 10;
+        memcpy(nfc_target.nfcid1, rsp->nfcid3t, nfc_target.nfcid1_len);
+        rc = nfc_targets_found(dev->nfc_dev, &nfc_target, 1);
+        if (rc)
+                goto error;
+
+        dev->tgt_available_prots = 0;
+        dev->tgt_active_prot = NFC_PROTO_NFC_DEP;
+
+        /* ATR_RES general bytes are located at offset 17 */
+        target_gt_len = resp->len - 17;
+        rc = nfc_set_remote_general_bytes(dev->nfc_dev,
+                                          rsp->gt, target_gt_len);
+        if (!rc) {
+                rc = nfc_dep_link_is_up(dev->nfc_dev,
+                                        dev->nfc_dev->targets[0].idx,
+                                        0, NFC_RF_INITIATOR);
+
+                if (!rc)
+                        pn533_poll_reset_mod_list(dev);
+        }
+error:
+        dev_kfree_skb(resp);
+        return rc;
+}
+
+#define PASSIVE_DATA_LEN 5
+static int pn533_poll_dep(struct nfc_dev *nfc_dev)
+{
+        struct pn533 *dev = nfc_get_drvdata(nfc_dev);
+        struct sk_buff *skb;
+        int rc, skb_len;
+        u8 *next, nfcid3[NFC_NFCID3_MAXSIZE];
+        u8 passive_data[PASSIVE_DATA_LEN] = {0x00, 0xff, 0xff, 0x00, 0x3};
+
+        dev_dbg(&dev->interface->dev, "%s", __func__);
+
+        if (!dev->gb) {
+                dev->gb = nfc_get_local_general_bytes(nfc_dev, &dev->gb_len);
+
+                if (!dev->gb || !dev->gb_len) {
+                        dev->poll_dep = 0;
+                        queue_work(dev->wq, &dev->rf_work);
+                }
+        }
+
+        skb_len = 3 + dev->gb_len; /* ActPass + BR + Next */
+        skb_len += PASSIVE_DATA_LEN;
+
+        /* NFCID3 */
+        skb_len += NFC_NFCID3_MAXSIZE;
+        nfcid3[0] = 0x1;
+        nfcid3[1] = 0xfe;
+        get_random_bytes(nfcid3 + 2, 6);
+
+        skb = pn533_alloc_skb(dev, skb_len);
+        if (!skb)
+                return -ENOMEM;
+
+        *skb_put(skb, 1) = 0x01;  /* Active */
+        *skb_put(skb, 1) = 0x02;  /* 424 kbps */
+
+        next = skb_put(skb, 1);  /* Next */
+        *next = 0;
+
+        /* Copy passive data */
+        memcpy(skb_put(skb, PASSIVE_DATA_LEN), passive_data, PASSIVE_DATA_LEN);
+        *next |= 1;
+
+        /* Copy NFCID3 (which is NFCID2 from SENSF_RES) */
+        memcpy(skb_put(skb, NFC_NFCID3_MAXSIZE), nfcid3,
+               NFC_NFCID3_MAXSIZE);
+        *next |= 2;
+
+        memcpy(skb_put(skb, dev->gb_len), dev->gb, dev->gb_len);
+        *next |= 4; /* We have some Gi */
+
+        rc = pn533_send_cmd_async(dev, PN533_CMD_IN_JUMP_FOR_DEP, skb,
+                                  pn533_poll_dep_complete, NULL);
+
+        if (rc < 0)
+                dev_kfree_skb(skb);
+
+        return rc;
+}
+
 static int pn533_poll_complete(struct pn533 *dev, void *arg,
                                struct sk_buff *resp)
 {
         struct pn533_poll_modulations *cur_mod;
         int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         if (IS_ERR(resp)) {
                 rc = PTR_ERR(resp);
 
-                nfc_dev_err(&dev->interface->dev, "%s  Poll complete error %d",
-                            __func__, rc);
+                nfc_err(&dev->interface->dev, "%s  Poll complete error %d\n",
+                        __func__, rc);
 
                 if (rc == -ENOENT) {
                         if (dev->poll_mod_count != 0)
@@ -1793,8 +1999,8 @@ static int pn533_poll_complete(struct pn533 *dev, void *arg,
                         else
                                 goto stop_poll;
                 } else if (rc < 0) {
-                        nfc_dev_err(&dev->interface->dev,
-                                    "Error %d when running poll", rc);
+                        nfc_err(&dev->interface->dev,
+                                "Error %d when running poll\n", rc);
                         goto stop_poll;
                 }
         }
@@ -1813,7 +2019,7 @@ static int pn533_poll_complete(struct pn533 *dev, void *arg,
                 goto done;
 
         if (!dev->poll_mod_count) {
-                nfc_dev_dbg(&dev->interface->dev, "Polling has been stopped.");
+                dev_dbg(&dev->interface->dev, "Polling has been stopped\n");
                 goto done;
         }
 
@@ -1826,7 +2032,7 @@ done:
         return rc;
 
 stop_poll:
-        nfc_dev_err(&dev->interface->dev, "Polling operation has been stopped");
+        nfc_err(&dev->interface->dev, "Polling operation has been stopped\n");
 
         pn533_poll_reset_mod_list(dev);
         dev->poll_protocols = 0;
@@ -1856,8 +2062,13 @@ static int pn533_send_poll_frame(struct pn533 *dev)
 
         mod = dev->poll_mod_active[dev->poll_mod_curr];
 
-        nfc_dev_dbg(&dev->interface->dev, "%s mod len %d\n",
-                    __func__, mod->len);
+        dev_dbg(&dev->interface->dev, "%s mod len %d\n",
+                __func__, mod->len);
+
+        if (dev->poll_dep)  {
+                dev->poll_dep = 0;
+                return pn533_poll_dep(dev->nfc_dev);
+        }
 
         if (mod->len == 0) {  /* Listen mode */
                 cmd_code = PN533_CMD_TG_INIT_AS_TARGET;
@@ -1868,7 +2079,7 @@ static int pn533_send_poll_frame(struct pn533 *dev)
         }
 
         if (!skb) {
-                nfc_dev_err(&dev->interface->dev, "Failed to allocate skb.");
+                nfc_err(&dev->interface->dev, "Failed to allocate skb\n");
                 return -ENOMEM;
         }
 
@@ -1876,7 +2087,7 @@ static int pn533_send_poll_frame(struct pn533 *dev)
                                   NULL);
         if (rc < 0) {
                 dev_kfree_skb(skb);
-                nfc_dev_err(&dev->interface->dev, "Polling loop error %d", rc);
+                nfc_err(&dev->interface->dev, "Polling loop error %d\n", rc);
         }
 
         return rc;
@@ -1890,9 +2101,9 @@ static void pn533_wq_poll(struct work_struct *work)
 
         cur_mod = dev->poll_mod_active[dev->poll_mod_curr];
 
-        nfc_dev_dbg(&dev->interface->dev,
-                    "%s cancel_listen %d modulation len %d",
-                    __func__, dev->cancel_listen, cur_mod->len);
+        dev_dbg(&dev->interface->dev,
+                "%s cancel_listen %d modulation len %d\n",
+                __func__, dev->cancel_listen, cur_mod->len);
 
         if (dev->cancel_listen == 1) {
                 dev->cancel_listen = 0;
@@ -1913,21 +2124,23 @@ static int pn533_start_poll(struct nfc_dev *nfc_dev,
                             u32 im_protocols, u32 tm_protocols)
 {
         struct pn533 *dev = nfc_get_drvdata(nfc_dev);
+        struct pn533_poll_modulations *cur_mod;
         u8 rand_mod;
+        int rc;
 
-        nfc_dev_dbg(&dev->interface->dev,
-                    "%s: im protocols 0x%x tm protocols 0x%x",
-                    __func__, im_protocols, tm_protocols);
+        dev_dbg(&dev->interface->dev,
+                "%s: im protocols 0x%x tm protocols 0x%x\n",
+                __func__, im_protocols, tm_protocols);
 
         if (dev->tgt_active_prot) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Cannot poll with a target already activated");
+                nfc_err(&dev->interface->dev,
+                        "Cannot poll with a target already activated\n");
                 return -EBUSY;
         }
 
         if (dev->tgt_mode) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Cannot poll while already being activated");
+                nfc_err(&dev->interface->dev,
+                        "Cannot poll while already being activated\n");
                 return -EBUSY;
         }
 
@@ -1946,20 +2159,26 @@ static int pn533_start_poll(struct nfc_dev *nfc_dev,
         rand_mod %= dev->poll_mod_count;
         dev->poll_mod_curr = rand_mod;
 
-        return pn533_send_poll_frame(dev);
+        cur_mod = dev->poll_mod_active[dev->poll_mod_curr];
+
+        rc = pn533_send_poll_frame(dev);
+
+        /* Start listen timer */
+        if (!rc && cur_mod->len == 0 && dev->poll_mod_count > 1)
+                mod_timer(&dev->listen_timer, jiffies + PN533_LISTEN_TIME * HZ);
+
+        return rc;
 }
 
 static void pn533_stop_poll(struct nfc_dev *nfc_dev)
 {
         struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
-
         del_timer(&dev->listen_timer);
 
         if (!dev->poll_mod_count) {
-                nfc_dev_dbg(&dev->interface->dev,
-                            "Polling operation was not running");
+                dev_dbg(&dev->interface->dev,
+                        "Polling operation was not running\n");
                 return;
         }
 
@@ -1973,11 +2192,10 @@ static int pn533_activate_target_nfcdep(struct pn533 *dev)
         struct pn533_cmd_activate_response *rsp;
         u16 gt_len;
         int rc;
-
         struct sk_buff *skb;
         struct sk_buff *resp;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         skb = pn533_alloc_skb(dev, sizeof(u8) * 2); /*TG + Next*/
         if (!skb)
@@ -1993,8 +2211,8 @@ static int pn533_activate_target_nfcdep(struct pn533 *dev)
         rsp = (struct pn533_cmd_activate_response *)resp->data;
         rc = rsp->status & PN533_CMD_RET_MASK;
         if (rc != PN533_CMD_RET_SUCCESS) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Target activation failed (error 0x%x)", rc);
+                nfc_err(&dev->interface->dev,
+                        "Target activation failed (error 0x%x)\n", rc);
                 dev_kfree_skb(resp);
                 return -EIO;
         }
@@ -2013,39 +2231,38 @@ static int pn533_activate_target(struct nfc_dev *nfc_dev,
         struct pn533 *dev = nfc_get_drvdata(nfc_dev);
         int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s - protocol=%u", __func__,
-                    protocol);
+        dev_dbg(&dev->interface->dev, "%s: protocol=%u\n", __func__, protocol);
 
         if (dev->poll_mod_count) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Cannot activate while polling");
+                nfc_err(&dev->interface->dev,
+                        "Cannot activate while polling\n");
                 return -EBUSY;
         }
 
         if (dev->tgt_active_prot) {
-                nfc_dev_err(&dev->interface->dev,
-                            "There is already an active target");
+                nfc_err(&dev->interface->dev,
+                        "There is already an active target\n");
                 return -EBUSY;
         }
 
         if (!dev->tgt_available_prots) {
-                nfc_dev_err(&dev->interface->dev,
-                            "There is no available target to activate");
+                nfc_err(&dev->interface->dev,
+                        "There is no available target to activate\n");
                 return -EINVAL;
         }
 
         if (!(dev->tgt_available_prots & (1 << protocol))) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Target doesn't support requested proto %u",
-                            protocol);
+                nfc_err(&dev->interface->dev,
+                        "Target doesn't support requested proto %u\n",
+                        protocol);
                 return -EINVAL;
         }
 
         if (protocol == NFC_PROTO_NFC_DEP) {
                 rc = pn533_activate_target_nfcdep(dev);
                 if (rc) {
-                        nfc_dev_err(&dev->interface->dev,
-                                    "Activating target with DEP failed %d", rc);
+                        nfc_err(&dev->interface->dev,
+                                "Activating target with DEP failed %d\n", rc);
                         return rc;
                 }
         }
@@ -2060,16 +2277,14 @@ static void pn533_deactivate_target(struct nfc_dev *nfc_dev,
                                     struct nfc_target *target)
 {
         struct pn533 *dev = nfc_get_drvdata(nfc_dev);
-
         struct sk_buff *skb;
         struct sk_buff *resp;
-
         int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         if (!dev->tgt_active_prot) {
-                nfc_dev_err(&dev->interface->dev, "There is no active target");
+                nfc_err(&dev->interface->dev, "There is no active target\n");
                 return;
         }
 
@@ -2088,8 +2303,8 @@ static void pn533_deactivate_target(struct nfc_dev *nfc_dev,
 
         rc = resp->data[0] & PN533_CMD_RET_MASK;
         if (rc != PN533_CMD_RET_SUCCESS)
-                nfc_dev_err(&dev->interface->dev,
-                            "Error 0x%x when releasing the target", rc);
+                nfc_err(&dev->interface->dev,
+                        "Error 0x%x when releasing the target\n", rc);
 
         dev_kfree_skb(resp);
         return;
@@ -2111,8 +2326,8 @@ static int pn533_in_dep_link_up_complete(struct pn533 *dev, void *arg,
 
         if (dev->tgt_available_prots &&
             !(dev->tgt_available_prots & (1 << NFC_PROTO_NFC_DEP))) {
-                nfc_dev_err(&dev->interface->dev,
-                            "The target does not support DEP");
+                nfc_err(&dev->interface->dev,
+                        "The target does not support DEP\n");
                 rc =  -EINVAL;
                 goto error;
         }
@@ -2121,15 +2336,15 @@ static int pn533_in_dep_link_up_complete(struct pn533 *dev, void *arg,
 
         rc = rsp->status & PN533_CMD_RET_MASK;
         if (rc != PN533_CMD_RET_SUCCESS) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Bringing DEP link up failed (error 0x%x)", rc);
+                nfc_err(&dev->interface->dev,
+                        "Bringing DEP link up failed (error 0x%x)\n", rc);
                 goto error;
         }
 
         if (!dev->tgt_available_prots) {
                 struct nfc_target nfc_target;
 
-                nfc_dev_dbg(&dev->interface->dev, "Creating new target");
+                dev_dbg(&dev->interface->dev, "Creating new target\n");
 
                 nfc_target.supported_protocols = NFC_PROTO_NFC_DEP_MASK;
                 nfc_target.nfcid1_len = 10;
@@ -2158,7 +2373,6 @@ error:
 }
 
 static int pn533_rf_field(struct nfc_dev *nfc_dev, u8 rf);
-#define PASSIVE_DATA_LEN 5
 static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
                              u8 comm_mode, u8 *gb, size_t gb_len)
 {
@@ -2166,20 +2380,19 @@ static int pn533_dep_link_up(struct nfc_dev *nfc_dev, struct nfc_target *target,
         struct sk_buff *skb;
         int rc, skb_len;
         u8 *next, *arg, nfcid3[NFC_NFCID3_MAXSIZE];
-
         u8 passive_data[PASSIVE_DATA_LEN] = {0x00, 0xff, 0xff, 0x00, 0x3};
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         if (dev->poll_mod_count) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Cannot bring the DEP link up while polling");
+                nfc_err(&dev->interface->dev,
+                        "Cannot bring the DEP link up while polling\n");
                 return -EBUSY;
         }
 
         if (dev->tgt_active_prot) {
-                nfc_dev_err(&dev->interface->dev,
-                            "There is already an active target");
+                nfc_err(&dev->interface->dev,
+                        "There is already an active target\n");
                 return -EBUSY;
         }
 
@@ -2249,7 +2462,7 @@ static int pn533_dep_link_down(struct nfc_dev *nfc_dev)
 {
         struct pn533 *dev = nfc_get_drvdata(nfc_dev);
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         pn533_poll_reset_mod_list(dev);
 
@@ -2274,7 +2487,7 @@ static struct sk_buff *pn533_build_response(struct pn533 *dev)
         struct sk_buff *skb, *tmp, *t;
         unsigned int skb_len = 0, tmp_len = 0;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         if (skb_queue_empty(&dev->resp_q))
                 return NULL;
@@ -2287,8 +2500,8 @@ static struct sk_buff *pn533_build_response(struct pn533 *dev)
         skb_queue_walk_safe(&dev->resp_q, tmp, t)
                 skb_len += tmp->len;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s total length %d\n",
-                    __func__, skb_len);
+        dev_dbg(&dev->interface->dev, "%s total length %d\n",
+                __func__, skb_len);
 
         skb = alloc_skb(skb_len, GFP_KERNEL);
         if (skb == NULL)
@@ -2315,7 +2528,7 @@ static int pn533_data_exchange_complete(struct pn533 *dev, void *_arg,
         int rc = 0;
         u8 status, ret, mi;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         if (IS_ERR(resp)) {
                 rc = PTR_ERR(resp);
@@ -2329,8 +2542,8 @@ static int pn533_data_exchange_complete(struct pn533 *dev, void *_arg,
         skb_pull(resp, sizeof(status));
 
         if (ret != PN533_CMD_RET_SUCCESS) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Exchanging data failed (error 0x%x)", ret);
+                nfc_err(&dev->interface->dev,
+                        "Exchanging data failed (error 0x%x)\n", ret);
                 rc = -EIO;
                 goto error;
         }
@@ -2388,14 +2601,17 @@ static int pn533_fill_fragment_skbs(struct pn533 *dev, struct sk_buff *skb)
                         break;
                 }
 
-                /* Reserve the TG/MI byte */
-                skb_reserve(frag, 1);
+                if (!dev->tgt_mode) {
+                        /* Reserve the TG/MI byte */
+                        skb_reserve(frag, 1);
 
-                /* MI + TG */
-                if (frag_size  == PN533_CMD_DATAFRAME_MAXLEN)
-                        *skb_push(frag, sizeof(u8)) = (PN533_CMD_MI_MASK | 1);
-                else
-                        *skb_push(frag, sizeof(u8)) =  1; /* TG */
+                        /* MI + TG */
+                        if (frag_size  == PN533_CMD_DATAFRAME_MAXLEN)
+                                *skb_push(frag, sizeof(u8)) =
+                                                        (PN533_CMD_MI_MASK | 1);
+                        else
+                                *skb_push(frag, sizeof(u8)) =  1; /* TG */
+                }
 
                 memcpy(skb_put(frag, frag_size), skb->data, frag_size);
 
@@ -2420,11 +2636,11 @@ static int pn533_transceive(struct nfc_dev *nfc_dev,
         struct pn533_data_exchange_arg *arg = NULL;
         int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         if (!dev->tgt_active_prot) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Can't exchange data if there is no active target");
+                nfc_err(&dev->interface->dev,
+                        "Can't exchange data if there is no active target\n");
                 rc = -EINVAL;
                 goto error;
         }
@@ -2487,13 +2703,18 @@ static int pn533_tm_send_complete(struct pn533 *dev, void *arg,
 {
         u8 status;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         if (IS_ERR(resp))
                 return PTR_ERR(resp);
 
         status = resp->data[0];
 
+        /* Prepare for the next round */
+        if (skb_queue_len(&dev->fragment_skb) > 0) {
+                queue_work(dev->wq, &dev->mi_tm_tx_work);
+                return -EINPROGRESS;
+        }
         dev_kfree_skb(resp);
 
         if (status != 0) {
@@ -2514,19 +2735,34 @@ static int pn533_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb)
         struct pn533 *dev = nfc_get_drvdata(nfc_dev);
         int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
+        /* let's split in multiple chunks if size's too big */
         if (skb->len > PN533_CMD_DATAEXCH_DATA_MAXLEN) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Data length greater than the max allowed: %d",
-                            PN533_CMD_DATAEXCH_DATA_MAXLEN);
-                return -ENOSYS;
+                rc = pn533_fill_fragment_skbs(dev, skb);
+                if (rc <= 0)
+                        goto error;
+
+                /* get the first skb */
+                skb = skb_dequeue(&dev->fragment_skb);
+                if (!skb) {
+                        rc = -EIO;
+                        goto error;
+                }
+
+                rc = pn533_send_data_async(dev, PN533_CMD_TG_SET_META_DATA, skb,
+                                                pn533_tm_send_complete, NULL);
+        } else {
+                /* Send th skb */
+                rc = pn533_send_data_async(dev, PN533_CMD_TG_SET_DATA, skb,
+                                                pn533_tm_send_complete, NULL);
         }
 
-        rc = pn533_send_data_async(dev, PN533_CMD_TG_SET_DATA, skb,
-                                   pn533_tm_send_complete, NULL);
-        if (rc < 0)
+error:
+        if (rc < 0) {
                 dev_kfree_skb(skb);
+                skb_queue_purge(&dev->fragment_skb);
+        }
 
         return rc;
 }
@@ -2534,11 +2770,10 @@ static int pn533_tm_send(struct nfc_dev *nfc_dev, struct sk_buff *skb)
 static void pn533_wq_mi_recv(struct work_struct *work)
 {
         struct pn533 *dev = container_of(work, struct pn533, mi_rx_work);
-
         struct sk_buff *skb;
         int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         skb = pn533_alloc_skb(dev, PN533_CMD_DATAEXCH_HEAD_LEN);
         if (!skb)
@@ -2570,8 +2805,8 @@ static void pn533_wq_mi_recv(struct work_struct *work)
         if (rc == 0) /* success */
                 return;
 
-        nfc_dev_err(&dev->interface->dev,
-                    "Error %d when trying to perform data_exchange", rc);
+        nfc_err(&dev->interface->dev,
+                "Error %d when trying to perform data_exchange\n", rc);
 
         dev_kfree_skb(skb);
         kfree(dev->cmd_complete_mi_arg);
@@ -2587,7 +2822,7 @@ static void pn533_wq_mi_send(struct work_struct *work)
         struct sk_buff *skb;
         int rc;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         /* Grab the first skb in the queue */
         skb = skb_dequeue(&dev->fragment_skb);
@@ -2625,8 +2860,8 @@ static void pn533_wq_mi_send(struct work_struct *work)
         if (rc == 0) /* success */
                 return;
 
-        nfc_dev_err(&dev->interface->dev,
-                    "Error %d when trying to perform data_exchange", rc);
+        nfc_err(&dev->interface->dev,
+                "Error %d when trying to perform data_exchange\n", rc);
 
         dev_kfree_skb(skb);
         kfree(dev->cmd_complete_dep_arg);
@@ -2641,10 +2876,9 @@ static int pn533_set_configuration(struct pn533 *dev, u8 cfgitem, u8 *cfgdata,
 {
         struct sk_buff *skb;
         struct sk_buff *resp;
-
         int skb_len;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         skb_len = sizeof(cfgitem) + cfgdata_len; /* cfgitem + cfgdata */
 
@@ -2691,7 +2925,7 @@ static int pn533_pasori_fw_reset(struct pn533 *dev)
         struct sk_buff *skb;
         struct sk_buff *resp;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         skb = pn533_alloc_skb(dev, sizeof(u8));
         if (!skb)
@@ -2717,7 +2951,7 @@ static void pn533_acr122_poweron_rdr_resp(struct urb *urb)
 {
         struct pn533_acr122_poweron_rdr_arg *arg = urb->context;
 
-        nfc_dev_dbg(&urb->dev->dev, "%s", __func__);
+        dev_dbg(&urb->dev->dev, "%s\n", __func__);
 
         print_hex_dump_debug("ACR122 RX: ", DUMP_PREFIX_NONE, 16, 1,
                        urb->transfer_buffer, urb->transfer_buffer_length,
@@ -2737,7 +2971,7 @@ static int pn533_acr122_poweron_rdr(struct pn533 *dev)
         void *cntx;
         struct pn533_acr122_poweron_rdr_arg arg;
 
-        nfc_dev_dbg(&dev->interface->dev, "%s", __func__);
+        dev_dbg(&dev->interface->dev, "%s\n", __func__);
 
         init_completion(&arg.done);
         cntx = dev->in_urb->context;  /* backup context */
@@ -2755,16 +2989,15 @@ static int pn533_acr122_poweron_rdr(struct pn533 *dev)
 
         rc = usb_submit_urb(dev->out_urb, GFP_KERNEL);
         if (rc) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Reader power on cmd error %d", rc);
+                nfc_err(&dev->interface->dev,
+                        "Reader power on cmd error %d\n", rc);
                 return rc;
         }
 
         rc =  usb_submit_urb(dev->in_urb, GFP_KERNEL);
         if (rc) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Can't submit for reader power on cmd response %d",
-                            rc);
+                nfc_err(&dev->interface->dev,
+                        "Can't submit reader poweron cmd response %d\n", rc);
                 return rc;
         }
 
@@ -2785,20 +3018,19 @@ static int pn533_rf_field(struct nfc_dev *nfc_dev, u8 rf)
         rc = pn533_set_configuration(dev, PN533_CFGITEM_RF_FIELD,
                                      (u8 *)&rf_field, 1);
         if (rc) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Error on setting RF field");
+                nfc_err(&dev->interface->dev, "Error on setting RF field\n");
                 return rc;
         }
 
         return rc;
 }
 
-int pn533_dev_up(struct nfc_dev *nfc_dev)
+static int pn533_dev_up(struct nfc_dev *nfc_dev)
 {
         return pn533_rf_field(nfc_dev, 1);
 }
 
-int pn533_dev_down(struct nfc_dev *nfc_dev)
+static int pn533_dev_down(struct nfc_dev *nfc_dev)
 {
         return pn533_rf_field(nfc_dev, 0);
 }
@@ -2839,16 +3071,16 @@ static int pn533_setup(struct pn533 *dev)
                 break;
 
         default:
-                nfc_dev_err(&dev->interface->dev, "Unknown device type %d\n",
-                            dev->device_type);
+                nfc_err(&dev->interface->dev, "Unknown device type %d\n",
+                        dev->device_type);
                 return -EINVAL;
         }
 
         rc = pn533_set_configuration(dev, PN533_CFGITEM_MAX_RETRIES,
                                      (u8 *)&max_retries, sizeof(max_retries));
         if (rc) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Error on setting MAX_RETRIES config");
+                nfc_err(&dev->interface->dev,
+                        "Error on setting MAX_RETRIES config\n");
                 return rc;
         }
 
@@ -2856,8 +3088,7 @@ static int pn533_setup(struct pn533 *dev)
         rc = pn533_set_configuration(dev, PN533_CFGITEM_TIMING,
                                      (u8 *)&timing, sizeof(timing));
         if (rc) {
-                nfc_dev_err(&dev->interface->dev,
-                            "Error on setting RF timings");
+                nfc_err(&dev->interface->dev, "Error on setting RF timings\n");
                 return rc;
         }
 
@@ -2871,8 +3102,8 @@ static int pn533_setup(struct pn533 *dev)
                 rc = pn533_set_configuration(dev, PN533_CFGITEM_PASORI,
                                              pasori_cfg, 3);
                 if (rc) {
-                        nfc_dev_err(&dev->interface->dev,
-                                    "Error while settings PASORI config");
+                        nfc_err(&dev->interface->dev,
+                                "Error while settings PASORI config\n");
                         return rc;
                 }
 
@@ -2917,8 +3148,8 @@ static int pn533_probe(struct usb_interface *interface,
         }
 
         if (!in_endpoint || !out_endpoint) {
-                nfc_dev_err(&interface->dev,
-                            "Could not find bulk-in or bulk-out endpoint");
+                nfc_err(&interface->dev,
+                        "Could not find bulk-in or bulk-out endpoint\n");
                 rc = -ENODEV;
                 goto error;
         }
@@ -2941,6 +3172,8 @@ static int pn533_probe(struct usb_interface *interface,
         INIT_WORK(&dev->mi_rx_work, pn533_wq_mi_recv);
         INIT_WORK(&dev->mi_tx_work, pn533_wq_mi_send);
         INIT_WORK(&dev->tg_work, pn533_wq_tg_get_data);
+        INIT_WORK(&dev->mi_tm_rx_work, pn533_wq_tm_mi_recv);
+        INIT_WORK(&dev->mi_tm_tx_work, pn533_wq_tm_mi_send);
         INIT_DELAYED_WORK(&dev->poll_work, pn533_wq_poll);
         INIT_WORK(&dev->rf_work, pn533_wq_rf);
         dev->wq = alloc_ordered_workqueue("pn533", 0);
@@ -2978,16 +3211,15 @@ static int pn533_probe(struct usb_interface *interface,
 
                 rc = pn533_acr122_poweron_rdr(dev);
                 if (rc < 0) {
-                        nfc_dev_err(&dev->interface->dev,
-                                    "Couldn't poweron the reader (error %d)",
-                                    rc);
+                        nfc_err(&dev->interface->dev,
+                                "Couldn't poweron the reader (error %d)\n", rc);
                         goto destroy_wq;
                 }
                 break;
 
         default:
-                nfc_dev_err(&dev->interface->dev, "Unknown device type %d\n",
-                            dev->device_type);
+                nfc_err(&dev->interface->dev, "Unknown device type %d\n",
+                        dev->device_type);
                 rc = -EINVAL;
                 goto destroy_wq;
         }
@@ -2997,9 +3229,9 @@ static int pn533_probe(struct usb_interface *interface,
         if (rc < 0)
                 goto destroy_wq;
 
-        nfc_dev_info(&dev->interface->dev,
-                     "NXP PN5%02X firmware ver %d.%d now attached",
-                     fw_ver.ic, fw_ver.ver, fw_ver.rev);
+        nfc_info(&dev->interface->dev,
+                 "NXP PN5%02X firmware ver %d.%d now attached\n",
+                 fw_ver.ic, fw_ver.ver, fw_ver.rev);
 
 
         dev->nfc_dev = nfc_allocate_device(&pn533_nfc_ops, protocols,
@@ -3070,7 +3302,7 @@ static void pn533_disconnect(struct usb_interface *interface)
         usb_free_urb(dev->out_urb);
         kfree(dev);
 
-        nfc_dev_info(&interface->dev, "NXP PN533 NFC device disconnected");
+        nfc_info(&interface->dev, "NXP PN533 NFC device disconnected\n");
 }
 
 static struct usb_driver pn533_driver = {
diff --git a/drivers/nfc/pn544/i2c.c b/drivers/nfc/pn544/i2c.c
index 01e27d4bdd0d..b158ee1c2ac6 100644
--- a/drivers/nfc/pn544/i2c.c
+++ b/drivers/nfc/pn544/i2c.c
@@ -18,6 +18,8 @@
  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/crc-ccitt.h>
 #include <linux/module.h>
 #include <linux/i2c.h>
@@ -151,8 +153,7 @@ static void pn544_hci_i2c_platform_init(struct pn544_i2c_phy *phy)
         char rset_cmd[] = { 0x05, 0xF9, 0x04, 0x00, 0xC3, 0xE5 };
         int count = sizeof(rset_cmd);
 
-        pr_info(DRIVER_DESC ": %s\n", __func__);
-        dev_info(&phy->i2c_dev->dev, "Detecting nfc_en polarity\n");
+        nfc_info(&phy->i2c_dev->dev, "Detecting nfc_en polarity\n");
 
         /* Disable fw download */
         gpio_set_value(phy->gpio_fw, 0);
@@ -173,7 +174,7 @@ static void pn544_hci_i2c_platform_init(struct pn544_i2c_phy *phy)
                         dev_dbg(&phy->i2c_dev->dev, "Sending reset cmd\n");
                         ret = i2c_master_send(phy->i2c_dev, rset_cmd, count);
                         if (ret == count) {
-                                dev_info(&phy->i2c_dev->dev,
+                                nfc_info(&phy->i2c_dev->dev,
                                          "nfc_en polarity : active %s\n",
                                          (polarity == 0 ? "low" : "high"));
                                 goto out;
@@ -181,7 +182,7 @@ static void pn544_hci_i2c_platform_init(struct pn544_i2c_phy *phy)
                 }
         }
 
-        dev_err(&phy->i2c_dev->dev,
+        nfc_err(&phy->i2c_dev->dev,
                 "Could not detect nfc_en polarity, fallback to active high\n");
 
 out:
@@ -201,7 +202,7 @@ static int pn544_hci_i2c_enable(void *phy_id)
 {
         struct pn544_i2c_phy *phy = phy_id;
 
-        pr_info(DRIVER_DESC ": %s\n", __func__);
+        pr_info("%s\n", __func__);
 
         pn544_hci_i2c_enable_mode(phy, PN544_HCI_MODE);
 
@@ -214,8 +215,6 @@ static void pn544_hci_i2c_disable(void *phy_id)
 {
         struct pn544_i2c_phy *phy = phy_id;
 
-        pr_info(DRIVER_DESC ": %s\n", __func__);
-
         gpio_set_value(phy->gpio_fw, 0);
         gpio_set_value(phy->gpio_en, !phy->en_polarity);
         usleep_range(10000, 15000);
@@ -298,11 +297,9 @@ static int check_crc(u8 *buf, int buflen)
         crc = ~crc;
 
         if (buf[len - 2] != (crc & 0xff) || buf[len - 1] != (crc >> 8)) {
-                pr_err(PN544_HCI_I2C_DRIVER_NAME
-                       ": CRC error 0x%x != 0x%x 0x%x\n",
+                pr_err("CRC error 0x%x != 0x%x 0x%x\n",
                        crc, buf[len - 1], buf[len - 2]);
-
-                pr_info(DRIVER_DESC ": %s : BAD CRC\n", __func__);
+                pr_info("%s: BAD CRC\n", __func__);
                 print_hex_dump(KERN_DEBUG, "crc: ", DUMP_PREFIX_NONE,
                                16, 2, buf, buflen, false);
                 return -EPERM;
@@ -328,13 +325,13 @@ static int pn544_hci_i2c_read(struct pn544_i2c_phy *phy, struct sk_buff **skb)
 
         r = i2c_master_recv(client, &len, 1);
         if (r != 1) {
-                dev_err(&client->dev, "cannot read len byte\n");
+                nfc_err(&client->dev, "cannot read len byte\n");
                 return -EREMOTEIO;
         }
 
         if ((len < (PN544_HCI_I2C_LLC_MIN_SIZE - 1)) ||
             (len > (PN544_HCI_I2C_LLC_MAX_SIZE - 1))) {
-                dev_err(&client->dev, "invalid len byte\n");
+                nfc_err(&client->dev, "invalid len byte\n");
                 r = -EBADMSG;
                 goto flush;
         }
@@ -386,7 +383,7 @@ static int pn544_hci_i2c_fw_read_status(struct pn544_i2c_phy *phy)
 
         r = i2c_master_recv(client, (char *) &response, sizeof(response));
         if (r != sizeof(response)) {
-                dev_err(&client->dev, "cannot read fw status\n");
+                nfc_err(&client->dev, "cannot read fw status\n");
                 return -EIO;
         }
 
@@ -478,8 +475,7 @@ static int pn544_hci_i2c_fw_download(void *phy_id, const char *firmware_name)
 {
         struct pn544_i2c_phy *phy = phy_id;
 
-        pr_info(DRIVER_DESC ": Starting Firmware Download (%s)\n",
-                firmware_name);
+        pr_info("Starting Firmware Download (%s)\n", firmware_name);
 
         strcpy(phy->firmware_name, firmware_name);
 
@@ -493,7 +489,7 @@ static int pn544_hci_i2c_fw_download(void *phy_id, const char *firmware_name)
 static void pn544_hci_i2c_fw_work_complete(struct pn544_i2c_phy *phy,
                                            int result)
 {
-        pr_info(DRIVER_DESC ": Firmware Download Complete, result=%d\n", result);
+        pr_info("Firmware Download Complete, result=%d\n", result);
 
         pn544_hci_i2c_disable(phy);
 
@@ -694,14 +690,14 @@ static int pn544_hci_i2c_probe(struct i2c_client *client,
         dev_dbg(&client->dev, "IRQ: %d\n", client->irq);
 
         if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
-                dev_err(&client->dev, "Need I2C_FUNC_I2C\n");
+                nfc_err(&client->dev, "Need I2C_FUNC_I2C\n");
                 return -ENODEV;
         }
 
         phy = devm_kzalloc(&client->dev, sizeof(struct pn544_i2c_phy),
                            GFP_KERNEL);
         if (!phy) {
-                dev_err(&client->dev,
+                nfc_err(&client->dev,
                         "Cannot allocate memory for pn544 i2c phy.\n");
                 return -ENOMEM;
         }
@@ -714,18 +710,18 @@ static int pn544_hci_i2c_probe(struct i2c_client *client,
 
         pdata = client->dev.platform_data;
         if (pdata == NULL) {
-                dev_err(&client->dev, "No platform data\n");
+                nfc_err(&client->dev, "No platform data\n");
                 return -EINVAL;
         }
 
         if (pdata->request_resources == NULL) {
-                dev_err(&client->dev, "request_resources() missing\n");
+                nfc_err(&client->dev, "request_resources() missing\n");
                 return -EINVAL;
         }
 
         r = pdata->request_resources(client);
         if (r) {
-                dev_err(&client->dev, "Cannot get platform resources\n");
+                nfc_err(&client->dev, "Cannot get platform resources\n");
                 return r;
         }
 
@@ -739,7 +735,7 @@ static int pn544_hci_i2c_probe(struct i2c_client *client,
                                  IRQF_TRIGGER_RISING | IRQF_ONESHOT,
                                  PN544_HCI_I2C_DRIVER_NAME, phy);
         if (r < 0) {
-                dev_err(&client->dev, "Unable to register IRQ handler\n");
+                nfc_err(&client->dev, "Unable to register IRQ handler\n");
                 goto err_rti;
         }
 
diff --git a/drivers/nfc/pn544/pn544.c b/drivers/nfc/pn544/pn544.c
index 078e62feba17..74cfa0a88b9e 100644
--- a/drivers/nfc/pn544/pn544.c
+++ b/drivers/nfc/pn544/pn544.c
@@ -18,6 +18,8 @@
  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/delay.h>
 #include <linux/slab.h>
 #include <linux/module.h>
@@ -41,6 +43,7 @@ enum pn544_state {
 
 /* Proprietary commands */
 #define PN544_WRITE             0x3f
+#define PN544_TEST_SWP          0x21
 
 /* Proprietary gates, events, commands and registers */
 
@@ -81,14 +84,17 @@ enum pn544_state {
 #define PN544_PL_NFCT_DEACTIVATED               0x09
 
 #define PN544_SWP_MGMT_GATE                     0xA0
+#define PN544_SWP_DEFAULT_MODE                  0x01
 
 #define PN544_NFC_WI_MGMT_GATE                  0xA1
+#define PN544_NFC_ESE_DEFAULT_MODE              0x01
 
 #define PN544_HCI_EVT_SND_DATA                  0x01
 #define PN544_HCI_EVT_ACTIVATED                 0x02
 #define PN544_HCI_EVT_DEACTIVATED               0x03
 #define PN544_HCI_EVT_RCV_DATA                  0x04
 #define PN544_HCI_EVT_CONTINUE_MI               0x05
+#define PN544_HCI_EVT_SWITCH_MODE               0x03
 
 #define PN544_HCI_CMD_ATTREQUEST                0x12
 #define PN544_HCI_CMD_CONTINUE_ACTIVATION       0x13
@@ -187,13 +193,6 @@ static int pn544_hci_ready(struct nfc_hci_dev *hdev)
 
                 {{0x9e, 0xb4}, 0x00},
 
-                {{0x9e, 0xd9}, 0xff},
-                {{0x9e, 0xda}, 0xff},
-                {{0x9e, 0xdb}, 0x23},
-                {{0x9e, 0xdc}, 0x21},
-                {{0x9e, 0xdd}, 0x22},
-                {{0x9e, 0xde}, 0x24},
-
                 {{0x9c, 0x01}, 0x08},
 
                 {{0x9e, 0xaa}, 0x01},
@@ -394,7 +393,7 @@ static int pn544_hci_start_poll(struct nfc_hci_dev *hdev,
         if ((im_protocols | tm_protocols) & NFC_PROTO_NFC_DEP_MASK) {
                 hdev->gb = nfc_get_local_general_bytes(hdev->ndev,
                                                         &hdev->gb_len);
-                pr_debug("generate local bytes %p", hdev->gb);
+                pr_debug("generate local bytes %p\n", hdev->gb);
                 if (hdev->gb == NULL || hdev->gb_len == 0) {
                         im_protocols &= ~NFC_PROTO_NFC_DEP_MASK;
                         tm_protocols &= ~NFC_PROTO_NFC_DEP_MASK;
@@ -696,7 +695,7 @@ static int pn544_hci_tm_send(struct nfc_hci_dev *hdev, struct sk_buff *skb)
 static int pn544_hci_check_presence(struct nfc_hci_dev *hdev,
                                    struct nfc_target *target)
 {
-        pr_debug("supported protocol %d", target->supported_protocols);
+        pr_debug("supported protocol %d\b", target->supported_protocols);
         if (target->supported_protocols & (NFC_PROTO_ISO14443_MASK |
                                         NFC_PROTO_ISO14443_B_MASK)) {
                 return nfc_hci_send_cmd(hdev, target->hci_reader_gate,
@@ -733,7 +732,7 @@ static int pn544_hci_event_received(struct nfc_hci_dev *hdev, u8 gate, u8 event,
         struct sk_buff *rgb_skb = NULL;
         int r;
 
-        pr_debug("hci event %d", event);
+        pr_debug("hci event %d\n", event);
         switch (event) {
         case PN544_HCI_EVT_ACTIVATED:
                 if (gate == PN544_RF_READER_NFCIP1_INITIATOR_GATE) {
@@ -764,7 +763,7 @@ static int pn544_hci_event_received(struct nfc_hci_dev *hdev, u8 gate, u8 event,
                 }
 
                 if (skb->data[0] != 0) {
-                        pr_debug("data0 %d", skb->data[0]);
+                        pr_debug("data0 %d\n", skb->data[0]);
                         r = -EPROTO;
                         goto exit;
                 }
@@ -792,6 +791,108 @@ static int pn544_hci_fw_download(struct nfc_hci_dev *hdev,
         return info->fw_download(info->phy_id, firmware_name);
 }
 
+static int pn544_hci_discover_se(struct nfc_hci_dev *hdev)
+{
+        u32 se_idx = 0;
+        u8 ese_mode = 0x01; /* Default mode */
+        struct sk_buff *res_skb;
+        int r;
+
+        r = nfc_hci_send_cmd(hdev, PN544_SYS_MGMT_GATE, PN544_TEST_SWP,
+                             NULL, 0, &res_skb);
+
+        if (r == 0) {
+                if (res_skb->len == 2 && res_skb->data[0] == 0x00)
+                        nfc_add_se(hdev->ndev, se_idx++, NFC_SE_UICC);
+
+                kfree_skb(res_skb);
+        }
+
+        r = nfc_hci_send_event(hdev, PN544_NFC_WI_MGMT_GATE,
+                                PN544_HCI_EVT_SWITCH_MODE,
+                                &ese_mode, 1);
+        if (r == 0)
+                nfc_add_se(hdev->ndev, se_idx++, NFC_SE_EMBEDDED);
+
+        return !se_idx;
+}
+
+#define PN544_SE_MODE_OFF       0x00
+#define PN544_SE_MODE_ON        0x01
+static int pn544_hci_enable_se(struct nfc_hci_dev *hdev, u32 se_idx)
+{
+        struct nfc_se *se;
+        u8 enable = PN544_SE_MODE_ON;
+        static struct uicc_gatelist {
+                u8 head;
+                u8 adr[2];
+                u8 value;
+        } uicc_gatelist[] = {
+                {0x00, {0x9e, 0xd9}, 0x23},
+                {0x00, {0x9e, 0xda}, 0x21},
+                {0x00, {0x9e, 0xdb}, 0x22},
+                {0x00, {0x9e, 0xdc}, 0x24},
+        };
+        struct uicc_gatelist *p = uicc_gatelist;
+        int count = ARRAY_SIZE(uicc_gatelist);
+        struct sk_buff *res_skb;
+        int r;
+
+        se = nfc_find_se(hdev->ndev, se_idx);
+
+        switch (se->type) {
+        case NFC_SE_UICC:
+                while (count--) {
+                        r = nfc_hci_send_cmd(hdev, PN544_SYS_MGMT_GATE,
+                                        PN544_WRITE, (u8 *)p, 4, &res_skb);
+                        if (r < 0)
+                                return r;
+
+                        if (res_skb->len != 1) {
+                                kfree_skb(res_skb);
+                                return -EPROTO;
+                        }
+
+                        if (res_skb->data[0] != p->value) {
+                                kfree_skb(res_skb);
+                                return -EIO;
+                        }
+
+                        kfree_skb(res_skb);
+
+                        p++;
+                }
+
+                return nfc_hci_set_param(hdev, PN544_SWP_MGMT_GATE,
+                              PN544_SWP_DEFAULT_MODE, &enable, 1);
+        case NFC_SE_EMBEDDED:
+                return nfc_hci_set_param(hdev, PN544_NFC_WI_MGMT_GATE,
+                              PN544_NFC_ESE_DEFAULT_MODE, &enable, 1);
+
+        default:
+                return -EINVAL;
+        }
+}
+
+static int pn544_hci_disable_se(struct nfc_hci_dev *hdev, u32 se_idx)
+{
+        struct nfc_se *se;
+        u8 disable = PN544_SE_MODE_OFF;
+
+        se = nfc_find_se(hdev->ndev, se_idx);
+
+        switch (se->type) {
+        case NFC_SE_UICC:
+                return nfc_hci_set_param(hdev, PN544_SWP_MGMT_GATE,
+                              PN544_SWP_DEFAULT_MODE, &disable, 1);
+        case NFC_SE_EMBEDDED:
+                return nfc_hci_set_param(hdev, PN544_NFC_WI_MGMT_GATE,
+                              PN544_NFC_ESE_DEFAULT_MODE, &disable, 1);
+        default:
+                return -EINVAL;
+        }
+}
+
 static struct nfc_hci_ops pn544_hci_ops = {
         .open = pn544_hci_open,
         .close = pn544_hci_close,
@@ -807,6 +908,9 @@ static struct nfc_hci_ops pn544_hci_ops = {
         .check_presence = pn544_hci_check_presence,
         .event_received = pn544_hci_event_received,
         .fw_download = pn544_hci_fw_download,
+        .discover_se = pn544_hci_discover_se,
+        .enable_se = pn544_hci_enable_se,
+        .disable_se = pn544_hci_disable_se,
 };
 
 int pn544_hci_probe(void *phy_id, struct nfc_phy_ops *phy_ops, char *llc_name,
@@ -820,7 +924,6 @@ int pn544_hci_probe(void *phy_id, struct nfc_phy_ops *phy_ops, char *llc_name,
 
         info = kzalloc(sizeof(struct pn544_hci_info), GFP_KERNEL);
         if (!info) {
-                pr_err("Cannot allocate memory for pn544_hci_info.\n");
                 r = -ENOMEM;
                 goto err_info_alloc;
         }
@@ -853,7 +956,7 @@ int pn544_hci_probe(void *phy_id, struct nfc_phy_ops *phy_ops, char *llc_name,
                                              phy_headroom + PN544_CMDS_HEADROOM,
                                              phy_tailroom, phy_payload);
         if (!info->hdev) {
-                pr_err("Cannot allocate nfc hdev.\n");
+                pr_err("Cannot allocate nfc hdev\n");
                 r = -ENOMEM;
                 goto err_alloc_hdev;
         }
diff --git a/drivers/nfc/port100.c b/drivers/nfc/port100.c
new file mode 100644
index 000000000000..8a0571eb2627
--- /dev/null
+++ b/drivers/nfc/port100.c
@@ -0,0 +1,1529 @@
+/*
+ * Sony NFC Port-100 Series driver
+ * Copyright (c) 2013, Intel Corporation.
+ *
+ * Partly based/Inspired by Stephen Tiedemann's nfcpy
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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 <linux/module.h>
+#include <linux/usb.h>
+#include <net/nfc/digital.h>
+
+#define VERSION "0.1"
+
+#define SONY_VENDOR_ID    0x054c
+#define RCS380_PRODUCT_ID 0x06c1
+
+#define PORT100_PROTOCOLS (NFC_PROTO_JEWEL_MASK    | \
+                           NFC_PROTO_MIFARE_MASK   | \
+                           NFC_PROTO_FELICA_MASK   | \
+                           NFC_PROTO_NFC_DEP_MASK)
+
+#define PORT100_CAPABILITIES (NFC_DIGITAL_DRV_CAPS_IN_CRC | \
+                              NFC_DIGITAL_DRV_CAPS_TG_CRC)
+
+/* Standard port100 frame definitions */
+#define PORT100_FRAME_HEADER_LEN (sizeof(struct port100_frame) \
+                                  + 2) /* data[0] CC, data[1] SCC */
+#define PORT100_FRAME_TAIL_LEN 2 /* data[len] DCS, data[len + 1] postamble*/
+
+#define PORT100_COMM_RF_HEAD_MAX_LEN (sizeof(struct port100_tg_comm_rf_cmd))
+
+/*
+ * Max extended frame payload len, excluding CC and SCC
+ * which are already in PORT100_FRAME_HEADER_LEN.
+ */
+#define PORT100_FRAME_MAX_PAYLOAD_LEN 1001
+
+#define PORT100_FRAME_ACK_SIZE 6 /* Preamble (1), SoPC (2), ACK Code (2),
+                                    Postamble (1) */
+static u8 ack_frame[PORT100_FRAME_ACK_SIZE] = {
+        0x00, 0x00, 0xff, 0x00, 0xff, 0x00
+};
+
+#define PORT100_FRAME_CHECKSUM(f) (f->data[le16_to_cpu(f->datalen)])
+#define PORT100_FRAME_POSTAMBLE(f) (f->data[le16_to_cpu(f->datalen) + 1])
+
+/* start of frame */
+#define PORT100_FRAME_SOF       0x00FF
+#define PORT100_FRAME_EXT       0xFFFF
+#define PORT100_FRAME_ACK       0x00FF
+
+/* Port-100 command: in or out */
+#define PORT100_FRAME_DIRECTION(f) (f->data[0]) /* CC */
+#define PORT100_FRAME_DIR_OUT 0xD6
+#define PORT100_FRAME_DIR_IN  0xD7
+
+/* Port-100 sub-command */
+#define PORT100_FRAME_CMD(f) (f->data[1]) /* SCC */
+
+#define PORT100_CMD_GET_FIRMWARE_VERSION 0x20
+#define PORT100_CMD_GET_COMMAND_TYPE     0x28
+#define PORT100_CMD_SET_COMMAND_TYPE     0x2A
+
+#define PORT100_CMD_IN_SET_RF       0x00
+#define PORT100_CMD_IN_SET_PROTOCOL 0x02
+#define PORT100_CMD_IN_COMM_RF      0x04
+
+#define PORT100_CMD_TG_SET_RF       0x40
+#define PORT100_CMD_TG_SET_PROTOCOL 0x42
+#define PORT100_CMD_TG_SET_RF_OFF   0x46
+#define PORT100_CMD_TG_COMM_RF      0x48
+
+#define PORT100_CMD_SWITCH_RF       0x06
+
+#define PORT100_CMD_RESPONSE(cmd) (cmd + 1)
+
+#define PORT100_CMD_TYPE_IS_SUPPORTED(mask, cmd_type) \
+        ((mask) & (0x01 << (cmd_type)))
+#define PORT100_CMD_TYPE_0      0
+#define PORT100_CMD_TYPE_1      1
+
+#define PORT100_CMD_STATUS_OK      0x00
+#define PORT100_CMD_STATUS_TIMEOUT 0x80
+
+#define PORT100_MDAA_TGT_HAS_BEEN_ACTIVATED_MASK 0x01
+#define PORT100_MDAA_TGT_WAS_ACTIVATED_MASK      0x02
+
+struct port100;
+
+typedef void (*port100_send_async_complete_t)(struct port100 *dev, void *arg,
+                                              struct sk_buff *resp);
+
+/**
+ * Setting sets structure for in_set_rf command
+ *
+ * @in_*_set_number: Represent the entry indexes in the port-100 RF Base Table.
+ *              This table contains multiple RF setting sets required for RF
+ *              communication.
+ *
+ * @in_*_comm_type: Theses fields set the communication type to be used.
+ */
+struct port100_in_rf_setting {
+        u8 in_send_set_number;
+        u8 in_send_comm_type;
+        u8 in_recv_set_number;
+        u8 in_recv_comm_type;
+} __packed;
+
+#define PORT100_COMM_TYPE_IN_212F 0x01
+#define PORT100_COMM_TYPE_IN_424F 0x02
+#define PORT100_COMM_TYPE_IN_106A 0x03
+
+static const struct port100_in_rf_setting in_rf_settings[] = {
+        [NFC_DIGITAL_RF_TECH_212F] = {
+                .in_send_set_number = 1,
+                .in_send_comm_type  = PORT100_COMM_TYPE_IN_212F,
+                .in_recv_set_number = 15,
+                .in_recv_comm_type  = PORT100_COMM_TYPE_IN_212F,
+        },
+        [NFC_DIGITAL_RF_TECH_424F] = {
+                .in_send_set_number = 1,
+                .in_send_comm_type  = PORT100_COMM_TYPE_IN_424F,
+                .in_recv_set_number = 15,
+                .in_recv_comm_type  = PORT100_COMM_TYPE_IN_424F,
+        },
+        [NFC_DIGITAL_RF_TECH_106A] = {
+                .in_send_set_number = 2,
+                .in_send_comm_type  = PORT100_COMM_TYPE_IN_106A,
+                .in_recv_set_number = 15,
+                .in_recv_comm_type  = PORT100_COMM_TYPE_IN_106A,
+        },
+};
+
+/**
+ * Setting sets structure for tg_set_rf command
+ *
+ * @tg_set_number: Represents the entry index in the port-100 RF Base Table.
+ *                 This table contains multiple RF setting sets required for RF
+ *                 communication. this field is used for both send and receive
+ *                 settings.
+ *
+ * @tg_comm_type: Sets the communication type to be used to send and receive
+ *                data.
+ */
+struct port100_tg_rf_setting {
+        u8 tg_set_number;
+        u8 tg_comm_type;
+} __packed;
+
+#define PORT100_COMM_TYPE_TG_106A 0x0B
+#define PORT100_COMM_TYPE_TG_212F 0x0C
+#define PORT100_COMM_TYPE_TG_424F 0x0D
+
+static const struct port100_tg_rf_setting tg_rf_settings[] = {
+        [NFC_DIGITAL_RF_TECH_106A] = {
+                .tg_set_number = 8,
+                .tg_comm_type = PORT100_COMM_TYPE_TG_106A,
+        },
+        [NFC_DIGITAL_RF_TECH_212F] = {
+                .tg_set_number = 8,
+                .tg_comm_type = PORT100_COMM_TYPE_TG_212F,
+        },
+        [NFC_DIGITAL_RF_TECH_424F] = {
+                .tg_set_number = 8,
+                .tg_comm_type = PORT100_COMM_TYPE_TG_424F,
+        },
+};
+
+#define PORT100_IN_PROT_INITIAL_GUARD_TIME      0x00
+#define PORT100_IN_PROT_ADD_CRC                 0x01
+#define PORT100_IN_PROT_CHECK_CRC               0x02
+#define PORT100_IN_PROT_MULTI_CARD              0x03
+#define PORT100_IN_PROT_ADD_PARITY              0x04
+#define PORT100_IN_PROT_CHECK_PARITY            0x05
+#define PORT100_IN_PROT_BITWISE_AC_RECV_MODE    0x06
+#define PORT100_IN_PROT_VALID_BIT_NUMBER        0x07
+#define PORT100_IN_PROT_CRYPTO1                 0x08
+#define PORT100_IN_PROT_ADD_SOF                 0x09
+#define PORT100_IN_PROT_CHECK_SOF               0x0A
+#define PORT100_IN_PROT_ADD_EOF                 0x0B
+#define PORT100_IN_PROT_CHECK_EOF               0x0C
+#define PORT100_IN_PROT_DEAF_TIME               0x0E
+#define PORT100_IN_PROT_CRM                     0x0F
+#define PORT100_IN_PROT_CRM_MIN_LEN             0x10
+#define PORT100_IN_PROT_T1_TAG_FRAME            0x11
+#define PORT100_IN_PROT_RFCA                    0x12
+#define PORT100_IN_PROT_GUARD_TIME_AT_INITIATOR 0x13
+#define PORT100_IN_PROT_END                     0x14
+
+#define PORT100_IN_MAX_NUM_PROTOCOLS            19
+
+#define PORT100_TG_PROT_TU           0x00
+#define PORT100_TG_PROT_RF_OFF       0x01
+#define PORT100_TG_PROT_CRM          0x02
+#define PORT100_TG_PROT_END          0x03
+
+#define PORT100_TG_MAX_NUM_PROTOCOLS 3
+
+struct port100_protocol {
+        u8 number;
+        u8 value;
+} __packed;
+
+static struct port100_protocol
+in_protocols[][PORT100_IN_MAX_NUM_PROTOCOLS + 1] = {
+        [NFC_DIGITAL_FRAMING_NFCA_SHORT] = {
+                { PORT100_IN_PROT_INITIAL_GUARD_TIME,      6 },
+                { PORT100_IN_PROT_ADD_CRC,                 0 },
+                { PORT100_IN_PROT_CHECK_CRC,               0 },
+                { PORT100_IN_PROT_MULTI_CARD,              0 },
+                { PORT100_IN_PROT_ADD_PARITY,              0 },
+                { PORT100_IN_PROT_CHECK_PARITY,            1 },
+                { PORT100_IN_PROT_BITWISE_AC_RECV_MODE,    0 },
+                { PORT100_IN_PROT_VALID_BIT_NUMBER,        7 },
+                { PORT100_IN_PROT_CRYPTO1,                 0 },
+                { PORT100_IN_PROT_ADD_SOF,                 0 },
+                { PORT100_IN_PROT_CHECK_SOF,               0 },
+                { PORT100_IN_PROT_ADD_EOF,                 0 },
+                { PORT100_IN_PROT_CHECK_EOF,               0 },
+                { PORT100_IN_PROT_DEAF_TIME,               4 },
+                { PORT100_IN_PROT_CRM,                     0 },
+                { PORT100_IN_PROT_CRM_MIN_LEN,             0 },
+                { PORT100_IN_PROT_T1_TAG_FRAME,            0 },
+                { PORT100_IN_PROT_RFCA,                    0 },
+                { PORT100_IN_PROT_GUARD_TIME_AT_INITIATOR, 6 },
+                { PORT100_IN_PROT_END,                     0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCA_STANDARD] = {
+                { PORT100_IN_PROT_INITIAL_GUARD_TIME,      6 },
+                { PORT100_IN_PROT_ADD_CRC,                 0 },
+                { PORT100_IN_PROT_CHECK_CRC,               0 },
+                { PORT100_IN_PROT_MULTI_CARD,              0 },
+                { PORT100_IN_PROT_ADD_PARITY,              1 },
+                { PORT100_IN_PROT_CHECK_PARITY,            1 },
+                { PORT100_IN_PROT_BITWISE_AC_RECV_MODE,    0 },
+                { PORT100_IN_PROT_VALID_BIT_NUMBER,        8 },
+                { PORT100_IN_PROT_CRYPTO1,                 0 },
+                { PORT100_IN_PROT_ADD_SOF,                 0 },
+                { PORT100_IN_PROT_CHECK_SOF,               0 },
+                { PORT100_IN_PROT_ADD_EOF,                 0 },
+                { PORT100_IN_PROT_CHECK_EOF,               0 },
+                { PORT100_IN_PROT_DEAF_TIME,               4 },
+                { PORT100_IN_PROT_CRM,                     0 },
+                { PORT100_IN_PROT_CRM_MIN_LEN,             0 },
+                { PORT100_IN_PROT_T1_TAG_FRAME,            0 },
+                { PORT100_IN_PROT_RFCA,                    0 },
+                { PORT100_IN_PROT_GUARD_TIME_AT_INITIATOR, 6 },
+                { PORT100_IN_PROT_END,                     0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCA_STANDARD_WITH_CRC_A] = {
+                { PORT100_IN_PROT_INITIAL_GUARD_TIME,      6 },
+                { PORT100_IN_PROT_ADD_CRC,                 1 },
+                { PORT100_IN_PROT_CHECK_CRC,               1 },
+                { PORT100_IN_PROT_MULTI_CARD,              0 },
+                { PORT100_IN_PROT_ADD_PARITY,              1 },
+                { PORT100_IN_PROT_CHECK_PARITY,            1 },
+                { PORT100_IN_PROT_BITWISE_AC_RECV_MODE,    0 },
+                { PORT100_IN_PROT_VALID_BIT_NUMBER,        8 },
+                { PORT100_IN_PROT_CRYPTO1,                 0 },
+                { PORT100_IN_PROT_ADD_SOF,                 0 },
+                { PORT100_IN_PROT_CHECK_SOF,               0 },
+                { PORT100_IN_PROT_ADD_EOF,                 0 },
+                { PORT100_IN_PROT_CHECK_EOF,               0 },
+                { PORT100_IN_PROT_DEAF_TIME,               4 },
+                { PORT100_IN_PROT_CRM,                     0 },
+                { PORT100_IN_PROT_CRM_MIN_LEN,             0 },
+                { PORT100_IN_PROT_T1_TAG_FRAME,            0 },
+                { PORT100_IN_PROT_RFCA,                    0 },
+                { PORT100_IN_PROT_GUARD_TIME_AT_INITIATOR, 6 },
+                { PORT100_IN_PROT_END,                     0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCA_T1T] = {
+                /* nfc_digital_framing_nfca_short */
+                { PORT100_IN_PROT_ADD_CRC,          2 },
+                { PORT100_IN_PROT_CHECK_CRC,        2 },
+                { PORT100_IN_PROT_VALID_BIT_NUMBER, 8 },
+                { PORT100_IN_PROT_T1_TAG_FRAME,     2 },
+                { PORT100_IN_PROT_END,              0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCA_T2T] = {
+                /* nfc_digital_framing_nfca_standard */
+                { PORT100_IN_PROT_ADD_CRC,   1 },
+                { PORT100_IN_PROT_CHECK_CRC, 0 },
+                { PORT100_IN_PROT_END,       0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCA_NFC_DEP] = {
+                /* nfc_digital_framing_nfca_standard */
+                { PORT100_IN_PROT_END, 0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCF] = {
+                { PORT100_IN_PROT_INITIAL_GUARD_TIME,     18 },
+                { PORT100_IN_PROT_ADD_CRC,                 1 },
+                { PORT100_IN_PROT_CHECK_CRC,               1 },
+                { PORT100_IN_PROT_MULTI_CARD,              0 },
+                { PORT100_IN_PROT_ADD_PARITY,              0 },
+                { PORT100_IN_PROT_CHECK_PARITY,            0 },
+                { PORT100_IN_PROT_BITWISE_AC_RECV_MODE,    0 },
+                { PORT100_IN_PROT_VALID_BIT_NUMBER,        8 },
+                { PORT100_IN_PROT_CRYPTO1,                 0 },
+                { PORT100_IN_PROT_ADD_SOF,                 0 },
+                { PORT100_IN_PROT_CHECK_SOF,               0 },
+                { PORT100_IN_PROT_ADD_EOF,                 0 },
+                { PORT100_IN_PROT_CHECK_EOF,               0 },
+                { PORT100_IN_PROT_DEAF_TIME,               4 },
+                { PORT100_IN_PROT_CRM,                     0 },
+                { PORT100_IN_PROT_CRM_MIN_LEN,             0 },
+                { PORT100_IN_PROT_T1_TAG_FRAME,            0 },
+                { PORT100_IN_PROT_RFCA,                    0 },
+                { PORT100_IN_PROT_GUARD_TIME_AT_INITIATOR, 6 },
+                { PORT100_IN_PROT_END,                     0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCF_T3T] = {
+                /* nfc_digital_framing_nfcf */
+                { PORT100_IN_PROT_END, 0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCF_NFC_DEP] = {
+                /* nfc_digital_framing_nfcf */
+                { PORT100_IN_PROT_END, 0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFC_DEP_ACTIVATED] = {
+                { PORT100_IN_PROT_END, 0 },
+        },
+};
+
+static struct port100_protocol
+tg_protocols[][PORT100_TG_MAX_NUM_PROTOCOLS + 1] = {
+        [NFC_DIGITAL_FRAMING_NFCA_SHORT] = {
+                { PORT100_TG_PROT_END, 0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCA_STANDARD] = {
+                { PORT100_TG_PROT_END, 0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCA_STANDARD_WITH_CRC_A] = {
+                { PORT100_TG_PROT_END, 0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCA_T1T] = {
+                { PORT100_TG_PROT_END, 0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCA_T2T] = {
+                { PORT100_TG_PROT_END, 0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCA_NFC_DEP] = {
+                { PORT100_TG_PROT_TU,     1 },
+                { PORT100_TG_PROT_RF_OFF, 0 },
+                { PORT100_TG_PROT_CRM,    7 },
+                { PORT100_TG_PROT_END,    0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCF] = {
+                { PORT100_TG_PROT_END, 0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCF_T3T] = {
+                { PORT100_TG_PROT_END, 0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFCF_NFC_DEP] = {
+                { PORT100_TG_PROT_TU,     1 },
+                { PORT100_TG_PROT_RF_OFF, 0 },
+                { PORT100_TG_PROT_CRM,    7 },
+                { PORT100_TG_PROT_END,    0 },
+        },
+        [NFC_DIGITAL_FRAMING_NFC_DEP_ACTIVATED] = {
+                { PORT100_TG_PROT_RF_OFF, 1 },
+                { PORT100_TG_PROT_END,    0 },
+        },
+};
+
+struct port100 {
+        struct nfc_digital_dev *nfc_digital_dev;
+
+        int skb_headroom;
+        int skb_tailroom;
+
+        struct usb_device *udev;
+        struct usb_interface *interface;
+
+        struct urb *out_urb;
+        struct urb *in_urb;
+
+        struct work_struct cmd_complete_work;
+
+        u8 cmd_type;
+
+        /* The digital stack serializes commands to be sent. There is no need
+         * for any queuing/locking mechanism at driver level.
+         */
+        struct port100_cmd *cmd;
+};
+
+struct port100_cmd {
+        u8 code;
+        int status;
+        struct sk_buff *req;
+        struct sk_buff *resp;
+        int resp_len;
+        port100_send_async_complete_t  complete_cb;
+        void *complete_cb_context;
+};
+
+struct port100_frame {
+        u8 preamble;
+        __be16 start_frame;
+        __be16 extended_frame;
+        __le16 datalen;
+        u8 datalen_checksum;
+        u8 data[];
+} __packed;
+
+struct port100_ack_frame {
+        u8 preamble;
+        __be16 start_frame;
+        __be16 ack_frame;
+        u8 postambule;
+} __packed;
+
+struct port100_cb_arg {
+        nfc_digital_cmd_complete_t complete_cb;
+        void *complete_arg;
+        u8 mdaa;
+};
+
+struct port100_tg_comm_rf_cmd {
+        __le16 guard_time;
+        __le16 send_timeout;
+        u8 mdaa;
+        u8 nfca_param[6];
+        u8 nfcf_param[18];
+        u8 mf_halted;
+        u8 arae_flag;
+        __le16 recv_timeout;
+        u8 data[];
+} __packed;
+
+struct port100_tg_comm_rf_res {
+        u8 comm_type;
+        u8 ar_status;
+        u8 target_activated;
+        __le32 status;
+        u8 data[];
+} __packed;
+
+/* The rule: value + checksum = 0 */
+static inline u8 port100_checksum(u16 value)
+{
+        return ~(((u8 *)&value)[0] + ((u8 *)&value)[1]) + 1;
+}
+
+/* The rule: sum(data elements) + checksum = 0 */
+static u8 port100_data_checksum(u8 *data, int datalen)
+{
+        u8 sum = 0;
+        int i;
+
+        for (i = 0; i < datalen; i++)
+                sum += data[i];
+
+        return port100_checksum(sum);
+}
+
+static void port100_tx_frame_init(void *_frame, u8 cmd_code)
+{
+        struct port100_frame *frame = _frame;
+
+        frame->preamble = 0;
+        frame->start_frame = cpu_to_be16(PORT100_FRAME_SOF);
+        frame->extended_frame = cpu_to_be16(PORT100_FRAME_EXT);
+        PORT100_FRAME_DIRECTION(frame) = PORT100_FRAME_DIR_OUT;
+        PORT100_FRAME_CMD(frame) = cmd_code;
+        frame->datalen = cpu_to_le16(2);
+}
+
+static void port100_tx_frame_finish(void *_frame)
+{
+        struct port100_frame *frame = _frame;
+
+        frame->datalen_checksum = port100_checksum(le16_to_cpu(frame->datalen));
+
+        PORT100_FRAME_CHECKSUM(frame) =
+                port100_data_checksum(frame->data, le16_to_cpu(frame->datalen));
+
+        PORT100_FRAME_POSTAMBLE(frame) = 0;
+}
+
+static void port100_tx_update_payload_len(void *_frame, int len)
+{
+        struct port100_frame *frame = _frame;
+
+        frame->datalen = cpu_to_le16(le16_to_cpu(frame->datalen) + len);
+}
+
+static bool port100_rx_frame_is_valid(void *_frame)
+{
+        u8 checksum;
+        struct port100_frame *frame = _frame;
+
+        if (frame->start_frame != cpu_to_be16(PORT100_FRAME_SOF) ||
+            frame->extended_frame != cpu_to_be16(PORT100_FRAME_EXT))
+                return false;
+
+        checksum = port100_checksum(le16_to_cpu(frame->datalen));
+        if (checksum != frame->datalen_checksum)
+                return false;
+
+        checksum = port100_data_checksum(frame->data,
+                                         le16_to_cpu(frame->datalen));
+        if (checksum != PORT100_FRAME_CHECKSUM(frame))
+                return false;
+
+        return true;
+}
+
+static bool port100_rx_frame_is_ack(struct port100_ack_frame *frame)
+{
+        return (frame->start_frame == cpu_to_be16(PORT100_FRAME_SOF) &&
+                frame->ack_frame == cpu_to_be16(PORT100_FRAME_ACK));
+}
+
+static inline int port100_rx_frame_size(void *frame)
+{
+        struct port100_frame *f = frame;
+
+        return sizeof(struct port100_frame) + le16_to_cpu(f->datalen) +
+               PORT100_FRAME_TAIL_LEN;
+}
+
+static bool port100_rx_frame_is_cmd_response(struct port100 *dev, void *frame)
+{
+        struct port100_frame *f = frame;
+
+        return (PORT100_FRAME_CMD(f) == PORT100_CMD_RESPONSE(dev->cmd->code));
+}
+
+static void port100_recv_response(struct urb *urb)
+{
+        struct port100 *dev = urb->context;
+        struct port100_cmd *cmd = dev->cmd;
+        u8 *in_frame;
+
+        cmd->status = urb->status;
+
+        switch (urb->status) {
+        case 0:
+                break; /* success */
+        case -ECONNRESET:
+        case -ENOENT:
+                nfc_err(&dev->interface->dev,
+                        "The urb has been canceled (status %d)", urb->status);
+                goto sched_wq;
+        case -ESHUTDOWN:
+        default:
+                nfc_err(&dev->interface->dev, "Urb failure (status %d)",
+                        urb->status);
+                goto sched_wq;
+        }
+
+        in_frame = dev->in_urb->transfer_buffer;
+
+        if (!port100_rx_frame_is_valid(in_frame)) {
+                nfc_err(&dev->interface->dev, "Received an invalid frame");
+                cmd->status = -EIO;
+                goto sched_wq;
+        }
+
+        print_hex_dump_debug("PORT100 RX: ", DUMP_PREFIX_NONE, 16, 1, in_frame,
+                             port100_rx_frame_size(in_frame), false);
+
+        if (!port100_rx_frame_is_cmd_response(dev, in_frame)) {
+                nfc_err(&dev->interface->dev,
+                        "It's not the response to the last command");
+                cmd->status = -EIO;
+                goto sched_wq;
+        }
+
+sched_wq:
+        schedule_work(&dev->cmd_complete_work);
+}
+
+static int port100_submit_urb_for_response(struct port100 *dev, gfp_t flags)
+{
+        dev->in_urb->complete = port100_recv_response;
+
+        return usb_submit_urb(dev->in_urb, flags);
+}
+
+static void port100_recv_ack(struct urb *urb)
+{
+        struct port100 *dev = urb->context;
+        struct port100_cmd *cmd = dev->cmd;
+        struct port100_ack_frame *in_frame;
+        int rc;
+
+        cmd->status = urb->status;
+
+        switch (urb->status) {
+        case 0:
+                break; /* success */
+        case -ECONNRESET:
+        case -ENOENT:
+                nfc_err(&dev->interface->dev,
+                        "The urb has been stopped (status %d)", urb->status);
+                goto sched_wq;
+        case -ESHUTDOWN:
+        default:
+                nfc_err(&dev->interface->dev, "Urb failure (status %d)",
+                        urb->status);
+                goto sched_wq;
+        }
+
+        in_frame = dev->in_urb->transfer_buffer;
+
+        if (!port100_rx_frame_is_ack(in_frame)) {
+                nfc_err(&dev->interface->dev, "Received an invalid ack");
+                cmd->status = -EIO;
+                goto sched_wq;
+        }
+
+        rc = port100_submit_urb_for_response(dev, GFP_ATOMIC);
+        if (rc) {
+                nfc_err(&dev->interface->dev,
+                        "usb_submit_urb failed with result %d", rc);
+                cmd->status = rc;
+                goto sched_wq;
+        }
+
+        return;
+
+sched_wq:
+        schedule_work(&dev->cmd_complete_work);
+}
+
+static int port100_submit_urb_for_ack(struct port100 *dev, gfp_t flags)
+{
+        dev->in_urb->complete = port100_recv_ack;
+
+        return usb_submit_urb(dev->in_urb, flags);
+}
+
+static int port100_send_ack(struct port100 *dev)
+{
+        int rc;
+
+        dev->out_urb->transfer_buffer = ack_frame;
+        dev->out_urb->transfer_buffer_length = sizeof(ack_frame);
+        rc = usb_submit_urb(dev->out_urb, GFP_KERNEL);
+
+        return rc;
+}
+
+static int port100_send_frame_async(struct port100 *dev, struct sk_buff *out,
+                                    struct sk_buff *in, int in_len)
+{
+        int rc;
+
+        dev->out_urb->transfer_buffer = out->data;
+        dev->out_urb->transfer_buffer_length = out->len;
+
+        dev->in_urb->transfer_buffer = in->data;
+        dev->in_urb->transfer_buffer_length = in_len;
+
+        print_hex_dump_debug("PORT100 TX: ", DUMP_PREFIX_NONE, 16, 1,
+                             out->data, out->len, false);
+
+        rc = usb_submit_urb(dev->out_urb, GFP_KERNEL);
+        if (rc)
+                return rc;
+
+        rc = port100_submit_urb_for_ack(dev, GFP_KERNEL);
+        if (rc)
+                goto error;
+
+        return 0;
+
+error:
+        usb_unlink_urb(dev->out_urb);
+        return rc;
+}
+
+static void port100_build_cmd_frame(struct port100 *dev, u8 cmd_code,
+                                    struct sk_buff *skb)
+{
+        /* payload is already there, just update datalen */
+        int payload_len = skb->len;
+
+        skb_push(skb, PORT100_FRAME_HEADER_LEN);
+        skb_put(skb, PORT100_FRAME_TAIL_LEN);
+
+        port100_tx_frame_init(skb->data, cmd_code);
+        port100_tx_update_payload_len(skb->data, payload_len);
+        port100_tx_frame_finish(skb->data);
+}
+
+static void port100_send_async_complete(struct port100 *dev)
+{
+        struct port100_cmd *cmd = dev->cmd;
+        int status = cmd->status;
+
+        struct sk_buff *req = cmd->req;
+        struct sk_buff *resp = cmd->resp;
+
+        dev_kfree_skb(req);
+
+        dev->cmd = NULL;
+
+        if (status < 0) {
+                cmd->complete_cb(dev, cmd->complete_cb_context,
+                                 ERR_PTR(status));
+                dev_kfree_skb(resp);
+                goto done;
+        }
+
+        skb_put(resp, port100_rx_frame_size(resp->data));
+        skb_pull(resp, PORT100_FRAME_HEADER_LEN);
+        skb_trim(resp, resp->len - PORT100_FRAME_TAIL_LEN);
+
+        cmd->complete_cb(dev, cmd->complete_cb_context, resp);
+
+done:
+        kfree(cmd);
+}
+
+static int port100_send_cmd_async(struct port100 *dev, u8 cmd_code,
+                                struct sk_buff *req,
+                                port100_send_async_complete_t complete_cb,
+                                void *complete_cb_context)
+{
+        struct port100_cmd *cmd;
+        struct sk_buff *resp;
+        int rc;
+        int  resp_len = PORT100_FRAME_HEADER_LEN +
+                        PORT100_FRAME_MAX_PAYLOAD_LEN +
+                        PORT100_FRAME_TAIL_LEN;
+
+        resp = alloc_skb(resp_len, GFP_KERNEL);
+        if (!resp)
+                return -ENOMEM;
+
+        cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+        if (!cmd) {
+                dev_kfree_skb(resp);
+                return -ENOMEM;
+        }
+
+        cmd->code = cmd_code;
+        cmd->req = req;
+        cmd->resp = resp;
+        cmd->resp_len = resp_len;
+        cmd->complete_cb = complete_cb;
+        cmd->complete_cb_context = complete_cb_context;
+
+        port100_build_cmd_frame(dev, cmd_code, req);
+
+        dev->cmd = cmd;
+
+        rc = port100_send_frame_async(dev, req, resp, resp_len);
+        if (rc) {
+                kfree(cmd);
+                dev_kfree_skb(resp);
+                dev->cmd = NULL;
+        }
+
+        return rc;
+}
+
+struct port100_sync_cmd_response {
+        struct sk_buff *resp;
+        struct completion done;
+};
+
+static void port100_wq_cmd_complete(struct work_struct *work)
+{
+        struct port100 *dev = container_of(work, struct port100,
+                                           cmd_complete_work);
+
+        port100_send_async_complete(dev);
+}
+
+static void port100_send_sync_complete(struct port100 *dev, void *_arg,
+                                      struct sk_buff *resp)
+{
+        struct port100_sync_cmd_response *arg = _arg;
+
+        arg->resp = resp;
+        complete(&arg->done);
+}
+
+static struct sk_buff *port100_send_cmd_sync(struct port100 *dev, u8 cmd_code,
+                                             struct sk_buff *req)
+{
+        int rc;
+        struct port100_sync_cmd_response arg;
+
+        init_completion(&arg.done);
+
+        rc = port100_send_cmd_async(dev, cmd_code, req,
+                                    port100_send_sync_complete, &arg);
+        if (rc) {
+                dev_kfree_skb(req);
+                return ERR_PTR(rc);
+        }
+
+        wait_for_completion(&arg.done);
+
+        return arg.resp;
+}
+
+static void port100_send_complete(struct urb *urb)
+{
+        struct port100 *dev = urb->context;
+
+        switch (urb->status) {
+        case 0:
+                break; /* success */
+        case -ECONNRESET:
+        case -ENOENT:
+                nfc_err(&dev->interface->dev,
+                        "The urb has been stopped (status %d)", urb->status);
+                break;
+        case -ESHUTDOWN:
+        default:
+                nfc_err(&dev->interface->dev, "Urb failure (status %d)",
+                        urb->status);
+        }
+}
+
+static void port100_abort_cmd(struct nfc_digital_dev *ddev)
+{
+        struct port100 *dev = nfc_digital_get_drvdata(ddev);
+
+        /* An ack will cancel the last issued command */
+        port100_send_ack(dev);
+
+        /* cancel the urb request */
+        usb_kill_urb(dev->in_urb);
+}
+
+static struct sk_buff *port100_alloc_skb(struct port100 *dev, unsigned int size)
+{
+        struct sk_buff *skb;
+
+        skb = alloc_skb(dev->skb_headroom + dev->skb_tailroom + size,
+                        GFP_KERNEL);
+        if (skb)
+                skb_reserve(skb, dev->skb_headroom);
+
+        return skb;
+}
+
+static int port100_set_command_type(struct port100 *dev, u8 command_type)
+{
+        struct sk_buff *skb;
+        struct sk_buff *resp;
+        int rc;
+
+        skb = port100_alloc_skb(dev, 1);
+        if (!skb)
+                return -ENOMEM;
+
+        *skb_put(skb, sizeof(u8)) = command_type;
+
+        resp = port100_send_cmd_sync(dev, PORT100_CMD_SET_COMMAND_TYPE, skb);
+        if (IS_ERR(resp))
+                return PTR_ERR(resp);
+
+        rc = resp->data[0];
+
+        dev_kfree_skb(resp);
+
+        return rc;
+}
+
+static u64 port100_get_command_type_mask(struct port100 *dev)
+{
+        struct sk_buff *skb;
+        struct sk_buff *resp;
+        u64 mask;
+
+        skb = port100_alloc_skb(dev, 0);
+        if (!skb)
+                return -ENOMEM;
+
+        resp = port100_send_cmd_sync(dev, PORT100_CMD_GET_COMMAND_TYPE, skb);
+        if (IS_ERR(resp))
+                return PTR_ERR(resp);
+
+        if (resp->len < 8)
+                mask = 0;
+        else
+                mask = be64_to_cpu(*(__be64 *)resp->data);
+
+        dev_kfree_skb(resp);
+
+        return mask;
+}
+
+static u16 port100_get_firmware_version(struct port100 *dev)
+{
+        struct sk_buff *skb;
+        struct sk_buff *resp;
+        u16 fw_ver;
+
+        skb = port100_alloc_skb(dev, 0);
+        if (!skb)
+                return 0;
+
+        resp = port100_send_cmd_sync(dev, PORT100_CMD_GET_FIRMWARE_VERSION,
+                                     skb);
+        if (IS_ERR(resp))
+                return 0;
+
+        fw_ver = le16_to_cpu(*(__le16 *)resp->data);
+
+        dev_kfree_skb(resp);
+
+        return fw_ver;
+}
+
+static int port100_switch_rf(struct nfc_digital_dev *ddev, bool on)
+{
+        struct port100 *dev = nfc_digital_get_drvdata(ddev);
+        struct sk_buff *skb, *resp;
+
+        skb = port100_alloc_skb(dev, 1);
+        if (!skb)
+                return -ENOMEM;
+
+        *skb_put(skb, 1) = on ? 1 : 0;
+
+        resp = port100_send_cmd_sync(dev, PORT100_CMD_SWITCH_RF, skb);
+
+        if (IS_ERR(resp))
+                return PTR_ERR(resp);
+
+        dev_kfree_skb(resp);
+
+        return 0;
+}
+
+static int port100_in_set_rf(struct nfc_digital_dev *ddev, u8 rf)
+{
+        struct port100 *dev = nfc_digital_get_drvdata(ddev);
+        struct sk_buff *skb;
+        struct sk_buff *resp;
+        int rc;
+
+        if (rf >= NFC_DIGITAL_RF_TECH_LAST)
+                return -EINVAL;
+
+        skb = port100_alloc_skb(dev, sizeof(struct port100_in_rf_setting));
+        if (!skb)
+                return -ENOMEM;
+
+        memcpy(skb_put(skb, sizeof(struct port100_in_rf_setting)),
+               &in_rf_settings[rf],
+               sizeof(struct port100_in_rf_setting));
+
+        resp = port100_send_cmd_sync(dev, PORT100_CMD_IN_SET_RF, skb);
+
+        if (IS_ERR(resp))
+                return PTR_ERR(resp);
+
+        rc = resp->data[0];
+
+        dev_kfree_skb(resp);
+
+        return rc;
+}
+
+static int port100_in_set_framing(struct nfc_digital_dev *ddev, int param)
+{
+        struct port100 *dev = nfc_digital_get_drvdata(ddev);
+        struct port100_protocol *protocols;
+        struct sk_buff *skb;
+        struct sk_buff *resp;
+        int num_protocols;
+        size_t size;
+        int rc;
+
+        if (param >= NFC_DIGITAL_FRAMING_LAST)
+                return -EINVAL;
+
+        protocols = in_protocols[param];
+
+        num_protocols = 0;
+        while (protocols[num_protocols].number != PORT100_IN_PROT_END)
+                num_protocols++;
+
+        if (!num_protocols)
+                return 0;
+
+        size = sizeof(struct port100_protocol) * num_protocols;
+
+        skb = port100_alloc_skb(dev, size);
+        if (!skb)
+                return -ENOMEM;
+
+        memcpy(skb_put(skb, size), protocols, size);
+
+        resp = port100_send_cmd_sync(dev, PORT100_CMD_IN_SET_PROTOCOL, skb);
+
+        if (IS_ERR(resp))
+                return PTR_ERR(resp);
+
+        rc = resp->data[0];
+
+        dev_kfree_skb(resp);
+
+        return rc;
+}
+
+static int port100_in_configure_hw(struct nfc_digital_dev *ddev, int type,
+                                   int param)
+{
+        if (type == NFC_DIGITAL_CONFIG_RF_TECH)
+                return port100_in_set_rf(ddev, param);
+
+        if (type == NFC_DIGITAL_CONFIG_FRAMING)
+                return port100_in_set_framing(ddev, param);
+
+        return -EINVAL;
+}
+
+static void port100_in_comm_rf_complete(struct port100 *dev, void *arg,
+                                       struct sk_buff *resp)
+{
+        struct port100_cb_arg *cb_arg = arg;
+        nfc_digital_cmd_complete_t cb = cb_arg->complete_cb;
+        u32 status;
+        int rc;
+
+        if (IS_ERR(resp)) {
+                rc =  PTR_ERR(resp);
+                goto exit;
+        }
+
+        if (resp->len < 4) {
+                nfc_err(&dev->interface->dev,
+                        "Invalid packet length received.\n");
+                rc = -EIO;
+                goto error;
+        }
+
+        status = le32_to_cpu(*(__le32 *)resp->data);
+
+        skb_pull(resp, sizeof(u32));
+
+        if (status == PORT100_CMD_STATUS_TIMEOUT) {
+                rc = -ETIMEDOUT;
+                goto error;
+        }
+
+        if (status != PORT100_CMD_STATUS_OK) {
+                nfc_err(&dev->interface->dev,
+                        "in_comm_rf failed with status 0x%08x\n", status);
+                rc = -EIO;
+                goto error;
+        }
+
+        /* Remove collision bits byte */
+        skb_pull(resp, 1);
+
+        goto exit;
+
+error:
+        kfree_skb(resp);
+        resp = ERR_PTR(rc);
+
+exit:
+        cb(dev->nfc_digital_dev, cb_arg->complete_arg, resp);
+
+        kfree(cb_arg);
+}
+
+static int port100_in_send_cmd(struct nfc_digital_dev *ddev,
+                               struct sk_buff *skb, u16 _timeout,
+                               nfc_digital_cmd_complete_t cb, void *arg)
+{
+        struct port100 *dev = nfc_digital_get_drvdata(ddev);
+        struct port100_cb_arg *cb_arg;
+        __le16 timeout;
+
+        cb_arg = kzalloc(sizeof(struct port100_cb_arg), GFP_KERNEL);
+        if (!cb_arg)
+                return -ENOMEM;
+
+        cb_arg->complete_cb = cb;
+        cb_arg->complete_arg = arg;
+
+        timeout = cpu_to_le16(_timeout * 10);
+
+        memcpy(skb_push(skb, sizeof(__le16)), &timeout, sizeof(__le16));
+
+        return port100_send_cmd_async(dev, PORT100_CMD_IN_COMM_RF, skb,
+                                      port100_in_comm_rf_complete, cb_arg);
+}
+
+static int port100_tg_set_rf(struct nfc_digital_dev *ddev, u8 rf)
+{
+        struct port100 *dev = nfc_digital_get_drvdata(ddev);
+        struct sk_buff *skb;
+        struct sk_buff *resp;
+        int rc;
+
+        if (rf >= NFC_DIGITAL_RF_TECH_LAST)
+                return -EINVAL;
+
+        skb = port100_alloc_skb(dev, sizeof(struct port100_tg_rf_setting));
+        if (!skb)
+                return -ENOMEM;
+
+        memcpy(skb_put(skb, sizeof(struct port100_tg_rf_setting)),
+               &tg_rf_settings[rf],
+               sizeof(struct port100_tg_rf_setting));
+
+        resp = port100_send_cmd_sync(dev, PORT100_CMD_TG_SET_RF, skb);
+
+        if (IS_ERR(resp))
+                return PTR_ERR(resp);
+
+        rc = resp->data[0];
+
+        dev_kfree_skb(resp);
+
+        return rc;
+}
+
+static int port100_tg_set_framing(struct nfc_digital_dev *ddev, int param)
+{
+        struct port100 *dev = nfc_digital_get_drvdata(ddev);
+        struct port100_protocol *protocols;
+        struct sk_buff *skb;
+        struct sk_buff *resp;
+        int rc;
+        int num_protocols;
+        size_t size;
+
+        if (param >= NFC_DIGITAL_FRAMING_LAST)
+                return -EINVAL;
+
+        protocols = tg_protocols[param];
+
+        num_protocols = 0;
+        while (protocols[num_protocols].number != PORT100_TG_PROT_END)
+                num_protocols++;
+
+        if (!num_protocols)
+                return 0;
+
+        size = sizeof(struct port100_protocol) * num_protocols;
+
+        skb = port100_alloc_skb(dev, size);
+        if (!skb)
+                return -ENOMEM;
+
+        memcpy(skb_put(skb, size), protocols, size);
+
+        resp = port100_send_cmd_sync(dev, PORT100_CMD_TG_SET_PROTOCOL, skb);
+
+        if (IS_ERR(resp))
+                return PTR_ERR(resp);
+
+        rc = resp->data[0];
+
+        dev_kfree_skb(resp);
+
+        return rc;
+}
+
+static int port100_tg_configure_hw(struct nfc_digital_dev *ddev, int type,
+                                   int param)
+{
+        if (type == NFC_DIGITAL_CONFIG_RF_TECH)
+                return port100_tg_set_rf(ddev, param);
+
+        if (type == NFC_DIGITAL_CONFIG_FRAMING)
+                return port100_tg_set_framing(ddev, param);
+
+        return -EINVAL;
+}
+
+static bool port100_tg_target_activated(struct port100 *dev, u8 tgt_activated)
+{
+        u8 mask;
+
+        switch (dev->cmd_type) {
+        case PORT100_CMD_TYPE_0:
+                mask = PORT100_MDAA_TGT_HAS_BEEN_ACTIVATED_MASK;
+                break;
+        case PORT100_CMD_TYPE_1:
+                mask = PORT100_MDAA_TGT_HAS_BEEN_ACTIVATED_MASK |
+                       PORT100_MDAA_TGT_WAS_ACTIVATED_MASK;
+                break;
+        default:
+                nfc_err(&dev->interface->dev, "Unknonwn command type.\n");
+                return false;
+        }
+
+        return ((tgt_activated & mask) == mask);
+}
+
+static void port100_tg_comm_rf_complete(struct port100 *dev, void *arg,
+                                        struct sk_buff *resp)
+{
+        u32 status;
+        struct port100_cb_arg *cb_arg = arg;
+        nfc_digital_cmd_complete_t cb = cb_arg->complete_cb;
+        struct port100_tg_comm_rf_res *hdr;
+
+        if (IS_ERR(resp))
+                goto exit;
+
+        hdr = (struct port100_tg_comm_rf_res *)resp->data;
+
+        status = le32_to_cpu(hdr->status);
+
+        if (cb_arg->mdaa &&
+            !port100_tg_target_activated(dev, hdr->target_activated)) {
+                kfree_skb(resp);
+                resp = ERR_PTR(-ETIMEDOUT);
+
+                goto exit;
+        }
+
+        skb_pull(resp, sizeof(struct port100_tg_comm_rf_res));
+
+        if (status != PORT100_CMD_STATUS_OK) {
+                kfree_skb(resp);
+
+                if (status == PORT100_CMD_STATUS_TIMEOUT)
+                        resp = ERR_PTR(-ETIMEDOUT);
+                else
+                        resp = ERR_PTR(-EIO);
+        }
+
+exit:
+        cb(dev->nfc_digital_dev, cb_arg->complete_arg, resp);
+
+        kfree(cb_arg);
+}
+
+static int port100_tg_send_cmd(struct nfc_digital_dev *ddev,
+                               struct sk_buff *skb, u16 timeout,
+                               nfc_digital_cmd_complete_t cb, void *arg)
+{
+        struct port100 *dev = nfc_digital_get_drvdata(ddev);
+        struct port100_tg_comm_rf_cmd *hdr;
+        struct port100_cb_arg *cb_arg;
+
+        cb_arg = kzalloc(sizeof(struct port100_cb_arg), GFP_KERNEL);
+        if (!cb_arg)
+                return -ENOMEM;
+
+        cb_arg->complete_cb = cb;
+        cb_arg->complete_arg = arg;
+
+        skb_push(skb, sizeof(struct port100_tg_comm_rf_cmd));
+
+        hdr = (struct port100_tg_comm_rf_cmd *)skb->data;
+
+        memset(hdr, 0, sizeof(struct port100_tg_comm_rf_cmd));
+        hdr->guard_time = cpu_to_le16(500);
+        hdr->send_timeout = cpu_to_le16(0xFFFF);
+        hdr->recv_timeout = cpu_to_le16(timeout);
+
+        return port100_send_cmd_async(dev, PORT100_CMD_TG_COMM_RF, skb,
+                                      port100_tg_comm_rf_complete, cb_arg);
+}
+
+static int port100_listen_mdaa(struct nfc_digital_dev *ddev,
+                               struct digital_tg_mdaa_params *params,
+                               u16 timeout,
+                               nfc_digital_cmd_complete_t cb, void *arg)
+{
+        struct port100 *dev = nfc_digital_get_drvdata(ddev);
+        struct port100_tg_comm_rf_cmd *hdr;
+        struct port100_cb_arg *cb_arg;
+        struct sk_buff *skb;
+        int rc;
+
+        rc = port100_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH,
+                                     NFC_DIGITAL_RF_TECH_106A);
+        if (rc)
+                return rc;
+
+        rc = port100_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+                                     NFC_DIGITAL_FRAMING_NFCA_NFC_DEP);
+        if (rc)
+                return rc;
+
+        cb_arg = kzalloc(sizeof(struct port100_cb_arg), GFP_KERNEL);
+        if (!cb_arg)
+                return -ENOMEM;
+
+        cb_arg->complete_cb = cb;
+        cb_arg->complete_arg = arg;
+        cb_arg->mdaa = 1;
+
+        skb = port100_alloc_skb(dev, 0);
+        if (!skb) {
+                kfree(cb_arg);
+                return -ENOMEM;
+        }
+
+        skb_push(skb, sizeof(struct port100_tg_comm_rf_cmd));
+        hdr = (struct port100_tg_comm_rf_cmd *)skb->data;
+
+        memset(hdr, 0, sizeof(struct port100_tg_comm_rf_cmd));
+
+        hdr->guard_time = 0;
+        hdr->send_timeout = cpu_to_le16(0xFFFF);
+        hdr->mdaa = 1;
+        hdr->nfca_param[0] = (params->sens_res >> 8) & 0xFF;
+        hdr->nfca_param[1] = params->sens_res & 0xFF;
+        memcpy(hdr->nfca_param + 2, params->nfcid1, 3);
+        hdr->nfca_param[5] = params->sel_res;
+        memcpy(hdr->nfcf_param, params->nfcid2, 8);
+        hdr->nfcf_param[16] = (params->sc >> 8) & 0xFF;
+        hdr->nfcf_param[17] = params->sc & 0xFF;
+        hdr->recv_timeout = cpu_to_le16(timeout);
+
+        return port100_send_cmd_async(dev, PORT100_CMD_TG_COMM_RF, skb,
+                                      port100_tg_comm_rf_complete, cb_arg);
+}
+
+static int port100_listen(struct nfc_digital_dev *ddev, u16 timeout,
+                          nfc_digital_cmd_complete_t cb, void *arg)
+{
+        struct port100 *dev = nfc_digital_get_drvdata(ddev);
+        struct sk_buff *skb;
+
+        skb = port100_alloc_skb(dev, 0);
+        if (!skb)
+                return -ENOMEM;
+
+        return port100_tg_send_cmd(ddev, skb, timeout, cb, arg);
+}
+
+static struct nfc_digital_ops port100_digital_ops = {
+        .in_configure_hw = port100_in_configure_hw,
+        .in_send_cmd = port100_in_send_cmd,
+
+        .tg_listen_mdaa = port100_listen_mdaa,
+        .tg_listen = port100_listen,
+        .tg_configure_hw = port100_tg_configure_hw,
+        .tg_send_cmd = port100_tg_send_cmd,
+
+        .switch_rf = port100_switch_rf,
+        .abort_cmd = port100_abort_cmd,
+};
+
+static const struct usb_device_id port100_table[] = {
+        { .match_flags          = USB_DEVICE_ID_MATCH_DEVICE,
+          .idVendor             = SONY_VENDOR_ID,
+          .idProduct            = RCS380_PRODUCT_ID,
+        },
+        { }
+};
+MODULE_DEVICE_TABLE(usb, port100_table);
+
+static int port100_probe(struct usb_interface *interface,
+                         const struct usb_device_id *id)
+{
+        struct port100 *dev;
+        int rc;
+        struct usb_host_interface *iface_desc;
+        struct usb_endpoint_descriptor *endpoint;
+        int in_endpoint;
+        int out_endpoint;
+        u16 fw_version;
+        u64 cmd_type_mask;
+        int i;
+
+        dev = devm_kzalloc(&interface->dev, sizeof(struct port100), GFP_KERNEL);
+        if (!dev)
+                return -ENOMEM;
+
+        dev->udev = usb_get_dev(interface_to_usbdev(interface));
+        dev->interface = interface;
+        usb_set_intfdata(interface, dev);
+
+        in_endpoint = out_endpoint = 0;
+        iface_desc = interface->cur_altsetting;
+        for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
+                endpoint = &iface_desc->endpoint[i].desc;
+
+                if (!in_endpoint && usb_endpoint_is_bulk_in(endpoint))
+                        in_endpoint = endpoint->bEndpointAddress;
+
+                if (!out_endpoint && usb_endpoint_is_bulk_out(endpoint))
+                        out_endpoint = endpoint->bEndpointAddress;
+        }
+
+        if (!in_endpoint || !out_endpoint) {
+                nfc_err(&interface->dev,
+                        "Could not find bulk-in or bulk-out endpoint\n");
+                rc = -ENODEV;
+                goto error;
+        }
+
+        dev->in_urb = usb_alloc_urb(0, GFP_KERNEL);
+        dev->out_urb = usb_alloc_urb(0, GFP_KERNEL);
+
+        if (!dev->in_urb || !dev->out_urb) {
+                nfc_err(&interface->dev, "Could not allocate USB URBs\n");
+                rc = -ENOMEM;
+                goto error;
+        }
+
+        usb_fill_bulk_urb(dev->in_urb, dev->udev,
+                          usb_rcvbulkpipe(dev->udev, in_endpoint),
+                          NULL, 0, NULL, dev);
+        usb_fill_bulk_urb(dev->out_urb, dev->udev,
+                          usb_sndbulkpipe(dev->udev, out_endpoint),
+                          NULL, 0, port100_send_complete, dev);
+
+        dev->skb_headroom = PORT100_FRAME_HEADER_LEN +
+                            PORT100_COMM_RF_HEAD_MAX_LEN;
+        dev->skb_tailroom = PORT100_FRAME_TAIL_LEN;
+
+        INIT_WORK(&dev->cmd_complete_work, port100_wq_cmd_complete);
+
+        /* The first thing to do with the Port-100 is to set the command type
+         * to be used. If supported we use command type 1. 0 otherwise.
+         */
+        cmd_type_mask = port100_get_command_type_mask(dev);
+        if (!cmd_type_mask) {
+                nfc_err(&interface->dev,
+                        "Could not get supported command types.\n");
+                rc = -ENODEV;
+                goto error;
+        }
+
+        if (PORT100_CMD_TYPE_IS_SUPPORTED(cmd_type_mask, PORT100_CMD_TYPE_1))
+                dev->cmd_type = PORT100_CMD_TYPE_1;
+        else
+                dev->cmd_type = PORT100_CMD_TYPE_0;
+
+        rc = port100_set_command_type(dev, dev->cmd_type);
+        if (rc) {
+                nfc_err(&interface->dev,
+                        "The device does not support command type %u.\n",
+                        dev->cmd_type);
+                goto error;
+        }
+
+        fw_version = port100_get_firmware_version(dev);
+        if (!fw_version)
+                nfc_err(&interface->dev,
+                        "Could not get device firmware version.\n");
+
+        nfc_info(&interface->dev,
+                 "Sony NFC Port-100 Series attached (firmware v%x.%02x)\n",
+                 (fw_version & 0xFF00) >> 8, fw_version & 0xFF);
+
+        dev->nfc_digital_dev = nfc_digital_allocate_device(&port100_digital_ops,
+                                                           PORT100_PROTOCOLS,
+                                                           PORT100_CAPABILITIES,
+                                                           dev->skb_headroom,
+                                                           dev->skb_tailroom);
+        if (!dev->nfc_digital_dev) {
+                nfc_err(&interface->dev,
+                        "Could not allocate nfc_digital_dev.\n");
+                rc = -ENOMEM;
+                goto error;
+        }
+
+        nfc_digital_set_parent_dev(dev->nfc_digital_dev, &interface->dev);
+        nfc_digital_set_drvdata(dev->nfc_digital_dev, dev);
+
+        rc = nfc_digital_register_device(dev->nfc_digital_dev);
+        if (rc) {
+                nfc_err(&interface->dev,
+                        "Could not register digital device.\n");
+                goto free_nfc_dev;
+        }
+
+        return 0;
+
+free_nfc_dev:
+        nfc_digital_free_device(dev->nfc_digital_dev);
+
+error:
+        usb_free_urb(dev->in_urb);
+        usb_free_urb(dev->out_urb);
+        usb_put_dev(dev->udev);
+
+        return rc;
+}
+
+static void port100_disconnect(struct usb_interface *interface)
+{
+        struct port100 *dev;
+
+        dev = usb_get_intfdata(interface);
+        usb_set_intfdata(interface, NULL);
+
+        nfc_digital_unregister_device(dev->nfc_digital_dev);
+        nfc_digital_free_device(dev->nfc_digital_dev);
+
+        usb_kill_urb(dev->in_urb);
+        usb_kill_urb(dev->out_urb);
+
+        usb_free_urb(dev->in_urb);
+        usb_free_urb(dev->out_urb);
+
+        kfree(dev->cmd);
+
+        nfc_info(&interface->dev, "Sony Port-100 NFC device disconnected");
+}
+
+static struct usb_driver port100_driver = {
+        .name =         "port100",
+        .probe =        port100_probe,
+        .disconnect =   port100_disconnect,
+        .id_table =     port100_table,
+};
+
+module_usb_driver(port100_driver);
+
+MODULE_DESCRIPTION("NFC Port-100 series usb driver ver " VERSION);
+MODULE_VERSION(VERSION);
+MODULE_LICENSE("GPL");
diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index d0de412bfa43..2cc9517fb0d5 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -218,11 +218,10 @@ void baswap(bdaddr_t *dst, bdaddr_t *src);
 
 struct bt_sock {
         struct sock sk;
-        bdaddr_t    src;
-        bdaddr_t    dst;
         struct list_head accept_q;
         struct sock *parent;
         unsigned long flags;
+        void (*skb_msg_name)(struct sk_buff *, void *, int *);
 };
 
 enum {
@@ -285,6 +284,8 @@ struct bt_skb_cb {
         __u8 force_active;
         struct l2cap_ctrl control;
         struct hci_req_ctrl req;
+        bdaddr_t bdaddr;
+        __le16 psm;
 };
 #define bt_cb(skb) ((struct bt_skb_cb *)((skb)->cb))
 
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index b90eec5e9c06..b096f5f73789 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -64,16 +64,20 @@
 #define HCI_AMP         0x01
 
 /* First BR/EDR Controller shall have ID = 0 */
-#define HCI_BREDR_ID    0
+#define AMP_ID_BREDR    0x00
+
+/* AMP controller types */
+#define AMP_TYPE_BREDR  0x00
+#define AMP_TYPE_80211  0x01
 
 /* AMP controller status */
-#define AMP_CTRL_POWERED_DOWN                   0x00
-#define AMP_CTRL_BLUETOOTH_ONLY                 0x01
-#define AMP_CTRL_NO_CAPACITY                    0x02
-#define AMP_CTRL_LOW_CAPACITY                   0x03
-#define AMP_CTRL_MEDIUM_CAPACITY                0x04
-#define AMP_CTRL_HIGH_CAPACITY                  0x05
-#define AMP_CTRL_FULL_CAPACITY                  0x06
+#define AMP_STATUS_POWERED_DOWN                 0x00
+#define AMP_STATUS_BLUETOOTH_ONLY               0x01
+#define AMP_STATUS_NO_CAPACITY                  0x02
+#define AMP_STATUS_LOW_CAPACITY                 0x03
+#define AMP_STATUS_MEDIUM_CAPACITY              0x04
+#define AMP_STATUS_HIGH_CAPACITY                0x05
+#define AMP_STATUS_FULL_CAPACITY                0x06
 
 /* HCI device quirks */
 enum {
@@ -118,7 +122,7 @@ enum {
         HCI_SSP_ENABLED,
         HCI_HS_ENABLED,
         HCI_LE_ENABLED,
-        HCI_LE_PERIPHERAL,
+        HCI_ADVERTISING,
         HCI_CONNECTABLE,
         HCI_DISCOVERABLE,
         HCI_LINK_SECURITY,
@@ -811,6 +815,14 @@ struct hci_cp_host_buffer_size {
         __le16   sco_max_pkt;
 } __packed;
 
+#define HCI_OP_READ_NUM_SUPPORTED_IAC   0x0c38
+struct hci_rp_read_num_supported_iac {
+        __u8    status;
+        __u8    num_iac;
+} __packed;
+
+#define HCI_OP_READ_CURRENT_IAC_LAP     0x0c39
+
 #define HCI_OP_WRITE_INQUIRY_MODE       0x0c45
 
 #define HCI_MAX_EIR_LENGTH              240
@@ -847,6 +859,8 @@ struct hci_rp_read_inq_rsp_tx_power {
 
 #define HCI_OP_SET_EVENT_MASK_PAGE_2    0x0c63
 
+#define HCI_OP_READ_LOCATION_DATA       0x0c64
+
 #define HCI_OP_READ_FLOW_CONTROL_MODE   0x0c66
 struct hci_rp_read_flow_control_mode {
         __u8     status;
@@ -1042,6 +1056,23 @@ struct hci_rp_le_read_local_features {
 
 #define HCI_OP_LE_SET_RANDOM_ADDR       0x2005
 
+#define LE_ADV_IND                      0x00
+#define LE_ADV_DIRECT_IND               0x01
+#define LE_ADV_SCAN_IND                 0x02
+#define LE_ADV_NONCONN_IND              0x03
+
+#define HCI_OP_LE_SET_ADV_PARAM         0x2006
+struct hci_cp_le_set_adv_param {
+        __le16   min_interval;
+        __le16   max_interval;
+        __u8     type;
+        __u8     own_address_type;
+        __u8     direct_addr_type;
+        bdaddr_t direct_addr;
+        __u8     channel_map;
+        __u8     filter_policy;
+} __packed;
+
 #define HCI_OP_LE_READ_ADV_TX_POWER     0x2007
 struct hci_rp_le_read_adv_tx_power {
         __u8    status;
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index e3ea48d6c992..2dc467939be7 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -159,11 +159,14 @@ struct hci_dev {
         __u16           manufacturer;
         __u16           lmp_subver;
         __u16           voice_setting;
+        __u8            num_iac;
         __u8            io_capability;
         __s8            inq_tx_power;
         __u16           page_scan_interval;
         __u16           page_scan_window;
         __u8            page_scan_type;
+        __u16           le_scan_interval;
+        __u16           le_scan_window;
 
         __u16           devid_source;
         __u16           devid_vendor;
@@ -285,9 +288,8 @@ struct hci_dev {
         int (*close)(struct hci_dev *hdev);
         int (*flush)(struct hci_dev *hdev);
         int (*setup)(struct hci_dev *hdev);
-        int (*send)(struct sk_buff *skb);
+        int (*send)(struct hci_dev *hdev, struct sk_buff *skb);
         void (*notify)(struct hci_dev *hdev, unsigned int evt);
-        int (*ioctl)(struct hci_dev *hdev, unsigned int cmd, unsigned long arg);
 };
 
 #define HCI_PHY_HANDLE(handle)  (handle & 0xff)
@@ -299,6 +301,8 @@ struct hci_conn {
 
         bdaddr_t        dst;
         __u8            dst_type;
+        bdaddr_t        src;
+        __u8            src_type;
         __u16           handle;
         __u16           state;
         __u8            mode;
@@ -703,19 +707,6 @@ static inline void hci_set_drvdata(struct hci_dev *hdev, void *data)
         dev_set_drvdata(&hdev->dev, data);
 }
 
-/* hci_dev_list shall be locked */
-static inline uint8_t __hci_num_ctrl(void)
-{
-        uint8_t count = 0;
-        struct list_head *p;
-
-        list_for_each(p, &hci_dev_list) {
-                count++;
-        }
-
-        return count;
-}
-
 struct hci_dev *hci_dev_get(int index);
 struct hci_dev *hci_get_route(bdaddr_t *dst, bdaddr_t *src);
 
@@ -768,7 +759,7 @@ int hci_remove_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr);
 
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb);
 
-int hci_recv_frame(struct sk_buff *skb);
+int hci_recv_frame(struct hci_dev *hdev, struct sk_buff *skb);
 int hci_recv_fragment(struct hci_dev *hdev, int type, void *data, int count);
 int hci_recv_stream_fragment(struct hci_dev *hdev, void *data, int count);
 
@@ -807,22 +798,6 @@ void hci_conn_del_sysfs(struct hci_conn *conn);
 #define lmp_host_le_capable(dev)   (!!((dev)->features[1][0] & LMP_HOST_LE))
 #define lmp_host_le_br_capable(dev) (!!((dev)->features[1][0] & LMP_HOST_LE_BREDR))
 
-/* returns true if at least one AMP active */
-static inline bool hci_amp_capable(void)
-{
-        struct hci_dev *hdev;
-        bool ret = false;
-
-        read_lock(&hci_dev_list_lock);
-        list_for_each_entry(hdev, &hci_dev_list, list)
-                if (hdev->amp_type == HCI_AMP &&
-                    test_bit(HCI_UP, &hdev->flags))
-                        ret = true;
-        read_unlock(&hci_dev_list_lock);
-
-        return ret;
-}
-
 /* ----- HCI protocols ----- */
 #define HCI_PROTO_DEFER             0x01
 
@@ -1120,24 +1095,24 @@ void hci_sock_dev_event(struct hci_dev *hdev, int event);
 #define DISCOV_BREDR_INQUIRY_LEN        0x08
 
 int mgmt_control(struct sock *sk, struct msghdr *msg, size_t len);
-int mgmt_index_added(struct hci_dev *hdev);
-int mgmt_index_removed(struct hci_dev *hdev);
-int mgmt_set_powered_failed(struct hci_dev *hdev, int err);
+void mgmt_index_added(struct hci_dev *hdev);
+void mgmt_index_removed(struct hci_dev *hdev);
+void mgmt_set_powered_failed(struct hci_dev *hdev, int err);
 int mgmt_powered(struct hci_dev *hdev, u8 powered);
 int mgmt_discoverable(struct hci_dev *hdev, u8 discoverable);
 int mgmt_connectable(struct hci_dev *hdev, u8 connectable);
 int mgmt_write_scan_failed(struct hci_dev *hdev, u8 scan, u8 status);
 int mgmt_new_link_key(struct hci_dev *hdev, struct link_key *key,
                       bool persistent);
-int mgmt_device_connected(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
-                          u8 addr_type, u32 flags, u8 *name, u8 name_len,
-                          u8 *dev_class);
-int mgmt_device_disconnected(struct hci_dev *hdev, bdaddr_t *bdaddr,
-                             u8 link_type, u8 addr_type, u8 reason);
-int mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr,
-                           u8 link_type, u8 addr_type, u8 status);
-int mgmt_connect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
-                        u8 addr_type, u8 status);
+void mgmt_device_connected(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
+                           u8 addr_type, u32 flags, u8 *name, u8 name_len,
+                           u8 *dev_class);
+void mgmt_device_disconnected(struct hci_dev *hdev, bdaddr_t *bdaddr,
+                              u8 link_type, u8 addr_type, u8 reason);
+void mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr,
+                            u8 link_type, u8 addr_type, u8 status);
+void mgmt_connect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
+                         u8 addr_type, u8 status);
 int mgmt_pin_code_request(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 secure);
 int mgmt_pin_code_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr,
                                  u8 status);
@@ -1168,16 +1143,16 @@ int mgmt_set_class_of_dev_complete(struct hci_dev *hdev, u8 *dev_class,
 int mgmt_set_local_name_complete(struct hci_dev *hdev, u8 *name, u8 status);
 int mgmt_read_local_oob_data_reply_complete(struct hci_dev *hdev, u8 *hash,
                                             u8 *randomizer, u8 status);
-int mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
-                      u8 addr_type, u8 *dev_class, s8 rssi, u8 cfm_name,
-                      u8 ssp, u8 *eir, u16 eir_len);
-int mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
-                     u8 addr_type, s8 rssi, u8 *name, u8 name_len);
-int mgmt_discovering(struct hci_dev *hdev, u8 discovering);
+void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
+                       u8 addr_type, u8 *dev_class, s8 rssi, u8 cfm_name,
+                       u8 ssp, u8 *eir, u16 eir_len);
+void mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
+                      u8 addr_type, s8 rssi, u8 *name, u8 name_len);
+void mgmt_discovering(struct hci_dev *hdev, u8 discovering);
 int mgmt_device_blocked(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type);
 int mgmt_device_unblocked(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type);
-bool mgmt_valid_hdev(struct hci_dev *hdev);
 int mgmt_new_ltk(struct hci_dev *hdev, struct smp_ltk *key, u8 persistent);
+void mgmt_reenable_advertising(struct hci_dev *hdev);
 
 /* HCI info for socket */
 #define hci_pi(sk) ((struct hci_pinfo *) sk)
@@ -1214,8 +1189,6 @@ void hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max,
 void hci_le_start_enc(struct hci_conn *conn, __le16 ediv, __u8 rand[8],
                                                         __u8 ltk[16]);
 
-u8 bdaddr_to_le(u8 bdaddr_type);
-
 #define SCO_AIRMODE_MASK       0x0003
 #define SCO_AIRMODE_CVSD       0x0000
 #define SCO_AIRMODE_TRANSP     0x0003
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index f141b5f6e4f1..07757a2af942 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -131,6 +131,7 @@ struct l2cap_conninfo {
 
 /* L2CAP fixed channels */
 #define L2CAP_FC_L2CAP          0x02
+#define L2CAP_FC_CONNLESS       0x04
 #define L2CAP_FC_A2MP           0x08
 
 /* L2CAP Control Field bit masks */
@@ -237,6 +238,7 @@ struct l2cap_conn_rsp {
 /* protocol/service multiplexer (PSM) */
 #define L2CAP_PSM_SDP           0x0001
 #define L2CAP_PSM_RFCOMM        0x0003
+#define L2CAP_PSM_3DSP          0x0021
 
 /* channel indentifier */
 #define L2CAP_CID_SIGNALING     0x0001
@@ -442,7 +444,12 @@ struct l2cap_chan {
 
         __u8            state;
 
+        bdaddr_t        dst;
+        __u8            dst_type;
+        bdaddr_t        src;
+        __u8            src_type;
         __le16          psm;
+        __le16          sport;
         __u16           dcid;
         __u16           scid;
 
@@ -453,8 +460,6 @@ struct l2cap_chan {
         __u8            chan_type;
         __u8            chan_policy;
 
-        __le16          sport;
-
         __u8            sec_level;
 
         __u8            ident;
@@ -549,6 +554,7 @@ struct l2cap_ops {
                                                  int state);
         void                    (*ready) (struct l2cap_chan *chan);
         void                    (*defer) (struct l2cap_chan *chan);
+        void                    (*resume) (struct l2cap_chan *chan);
         struct sk_buff          *(*alloc_skb) (struct l2cap_chan *chan,
                                                unsigned long len, int nb);
 };
@@ -557,9 +563,6 @@ struct l2cap_conn {
         struct hci_conn         *hcon;
         struct hci_chan         *hchan;
 
-        bdaddr_t                *dst;
-        bdaddr_t                *src;
-
         unsigned int            mtu;
 
         __u32                   feat_mask;
@@ -650,6 +653,7 @@ enum {
         FLAG_FLUSHABLE,
         FLAG_EXT_CTRL,
         FLAG_EFS_ENABLE,
+        FLAG_DEFER_SETUP,
 };
 
 enum {
diff --git a/include/net/bluetooth/mgmt.h b/include/net/bluetooth/mgmt.h
index 2ad433bb9a2e..518c5c84e39a 100644
--- a/include/net/bluetooth/mgmt.h
+++ b/include/net/bluetooth/mgmt.h
@@ -362,6 +362,13 @@ struct mgmt_cp_set_static_address {
 } __packed;
 #define MGMT_SET_STATIC_ADDRESS_SIZE    6
 
+#define MGMT_OP_SET_SCAN_PARAMS         0x002C
+struct mgmt_cp_set_scan_params {
+        __le16  interval;
+        __le16  window;
+} __packed;
+#define MGMT_SET_SCAN_PARAMS_SIZE       4
+
 #define MGMT_EV_CMD_COMPLETE            0x0001
 struct mgmt_ev_cmd_complete {
         __le16  opcode;
diff --git a/include/net/bluetooth/rfcomm.h b/include/net/bluetooth/rfcomm.h
index b7f43e76a5c5..486213a1aed8 100644
--- a/include/net/bluetooth/rfcomm.h
+++ b/include/net/bluetooth/rfcomm.h
@@ -300,6 +300,8 @@ struct rfcomm_conninfo {
 
 struct rfcomm_pinfo {
         struct bt_sock bt;
+        bdaddr_t src;
+        bdaddr_t dst;
         struct rfcomm_dlc   *dlc;
         u8     channel;
         u8     sec_level;
diff --git a/include/net/bluetooth/sco.h b/include/net/bluetooth/sco.h
index e252a31ee6b6..2019d1a0996a 100644
--- a/include/net/bluetooth/sco.h
+++ b/include/net/bluetooth/sco.h
@@ -55,9 +55,6 @@ struct sco_conninfo {
 struct sco_conn {
         struct hci_conn *hcon;
 
-        bdaddr_t        *dst;
-        bdaddr_t        *src;
-
         spinlock_t      lock;
         struct sock     *sk;
 
@@ -72,6 +69,8 @@ struct sco_conn {
 
 struct sco_pinfo {
         struct bt_sock  bt;
+        bdaddr_t        src;
+        bdaddr_t        dst;
         __u32           flags;
         __u16           setting;
         struct sco_conn *conn;
diff --git a/include/net/cfg80211.h b/include/net/cfg80211.h
index 45f6bf591104..419202ce3f95 100644
--- a/include/net/cfg80211.h
+++ b/include/net/cfg80211.h
@@ -3483,6 +3483,15 @@ void wiphy_apply_custom_regulatory(struct wiphy *wiphy,
 const struct ieee80211_reg_rule *freq_reg_info(struct wiphy *wiphy,
                                                u32 center_freq);
 
+/**
+ * reg_initiator_name - map regulatory request initiator enum to name
+ * @initiator: the regulatory request initiator
+ *
+ * You can use this to map the regulatory request initiator enum to a
+ * proper string representation.
+ */
+const char *reg_initiator_name(enum nl80211_reg_initiator initiator);
+
 /*
  * callbacks for asynchronous cfg80211 methods, notification
  * functions and BSS handling helpers
diff --git a/include/net/nfc/digital.h b/include/net/nfc/digital.h
new file mode 100644
index 000000000000..36acecd5f06c
--- /dev/null
+++ b/include/net/nfc/digital.h
@@ -0,0 +1,227 @@
+/*
+ * NFC Digital Protocol stack
+ * Copyright (c) 2013, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ *
+ */
+
+#ifndef __NFC_DIGITAL_H
+#define __NFC_DIGITAL_H
+
+#include <linux/skbuff.h>
+#include <net/nfc/nfc.h>
+
+/**
+ * Configuration types for in_configure_hw and tg_configure_hw.
+ */
+enum {
+        NFC_DIGITAL_CONFIG_RF_TECH = 0,
+        NFC_DIGITAL_CONFIG_FRAMING,
+};
+
+/**
+ * RF technology values passed as param argument to in_configure_hw and
+ * tg_configure_hw for NFC_DIGITAL_CONFIG_RF_TECH configuration type.
+ */
+enum {
+        NFC_DIGITAL_RF_TECH_106A = 0,
+        NFC_DIGITAL_RF_TECH_212F,
+        NFC_DIGITAL_RF_TECH_424F,
+
+        NFC_DIGITAL_RF_TECH_LAST,
+};
+
+/**
+ * Framing configuration passed as param argument to in_configure_hw and
+ * tg_configure_hw for NFC_DIGITAL_CONFIG_FRAMING configuration type.
+ */
+enum {
+        NFC_DIGITAL_FRAMING_NFCA_SHORT = 0,
+        NFC_DIGITAL_FRAMING_NFCA_STANDARD,
+        NFC_DIGITAL_FRAMING_NFCA_STANDARD_WITH_CRC_A,
+
+        NFC_DIGITAL_FRAMING_NFCA_T1T,
+        NFC_DIGITAL_FRAMING_NFCA_T2T,
+        NFC_DIGITAL_FRAMING_NFCA_NFC_DEP,
+
+        NFC_DIGITAL_FRAMING_NFCF,
+        NFC_DIGITAL_FRAMING_NFCF_T3T,
+        NFC_DIGITAL_FRAMING_NFCF_NFC_DEP,
+        NFC_DIGITAL_FRAMING_NFC_DEP_ACTIVATED,
+
+        NFC_DIGITAL_FRAMING_LAST,
+};
+
+#define DIGITAL_MDAA_NFCID1_SIZE 3
+
+struct digital_tg_mdaa_params {
+        u16 sens_res;
+        u8 nfcid1[DIGITAL_MDAA_NFCID1_SIZE];
+        u8 sel_res;
+
+        u8 nfcid2[NFC_NFCID2_MAXSIZE];
+        u16 sc;
+};
+
+struct nfc_digital_dev;
+
+/**
+ * nfc_digital_cmd_complete_t - Definition of command result callback
+ *
+ * @ddev: nfc_digital_device ref
+ * @arg: user data
+ * @resp: response data
+ *
+ * resp pointer can be an error code and will be checked with IS_ERR() macro.
+ * The callback is responsible for freeing resp sk_buff.
+ */
+typedef void (*nfc_digital_cmd_complete_t)(struct nfc_digital_dev *ddev,
+                                           void *arg, struct sk_buff *resp);
+
+/**
+ * Device side NFC Digital operations
+ *
+ * Initiator mode:
+ * @in_configure_hw: Hardware configuration for RF technology and communication
+ *      framing in initiator mode. This is a synchronous function.
+ * @in_send_cmd: Initiator mode data exchange using RF technology and framing
+ *      previously set with in_configure_hw. The peer response is returned
+ *      through callback cb. If an io error occurs or the peer didn't reply
+ *      within the specified timeout (ms), the error code is passed back through
+ *      the resp pointer. This is an asynchronous function.
+ *
+ * Target mode: Only NFC-DEP protocol is supported in target mode.
+ * @tg_configure_hw: Hardware configuration for RF technology and communication
+ *      framing in target mode. This is a synchronous function.
+ * @tg_send_cmd: Target mode data exchange using RF technology and framing
+ *      previously set with tg_configure_hw. The peer next command is returned
+ *      through callback cb. If an io error occurs or the peer didn't reply
+ *      within the specified timeout (ms), the error code is passed back through
+ *      the resp pointer. This is an asynchronous function.
+ * @tg_listen: Put the device in listen mode waiting for data from the peer
+ *      device. This is an asynchronous function.
+ * @tg_listen_mdaa: If supported, put the device in automatic listen mode with
+ *      mode detection and automatic anti-collision. In this mode, the device
+ *      automatically detects the RF technology and executes the anti-collision
+ *      detection using the command responses specified in mdaa_params. The
+ *      mdaa_params structure contains SENS_RES, NFCID1, and SEL_RES for 106A RF
+ *      tech. NFCID2 and system code (sc) for 212F and 424F. The driver returns
+ *      the NFC-DEP ATR_REQ command through cb. The digital stack deducts the RF
+ *      tech by analyzing the SoD of the frame containing the ATR_REQ command.
+ *      This is an asynchronous function.
+ *
+ * @switch_rf: Turns device radio on or off. The stack does not call explicitly
+ *      switch_rf to turn the radio on. A call to in|tg_configure_hw must turn
+ *      the device radio on.
+ * @abort_cmd: Discard the last sent command.
+ */
+struct nfc_digital_ops {
+        int (*in_configure_hw)(struct nfc_digital_dev *ddev, int type,
+                               int param);
+        int (*in_send_cmd)(struct nfc_digital_dev *ddev, struct sk_buff *skb,
+                           u16 timeout, nfc_digital_cmd_complete_t cb,
+                           void *arg);
+
+        int (*tg_configure_hw)(struct nfc_digital_dev *ddev, int type,
+                               int param);
+        int (*tg_send_cmd)(struct nfc_digital_dev *ddev, struct sk_buff *skb,
+                           u16 timeout, nfc_digital_cmd_complete_t cb,
+                           void *arg);
+        int (*tg_listen)(struct nfc_digital_dev *ddev, u16 timeout,
+                         nfc_digital_cmd_complete_t cb, void *arg);
+        int (*tg_listen_mdaa)(struct nfc_digital_dev *ddev,
+                              struct digital_tg_mdaa_params *mdaa_params,
+                              u16 timeout, nfc_digital_cmd_complete_t cb,
+                              void *arg);
+
+        int (*switch_rf)(struct nfc_digital_dev *ddev, bool on);
+        void (*abort_cmd)(struct nfc_digital_dev *ddev);
+};
+
+#define NFC_DIGITAL_POLL_MODE_COUNT_MAX 6 /* 106A, 212F, and 424F in & tg */
+
+typedef int (*digital_poll_t)(struct nfc_digital_dev *ddev, u8 rf_tech);
+
+struct digital_poll_tech {
+        u8 rf_tech;
+        digital_poll_t poll_func;
+};
+
+/**
+ * Driver capabilities - bit mask made of the following values
+ *
+ * @NFC_DIGITAL_DRV_CAPS_IN_CRC: The driver handles CRC calculation in initiator
+ *      mode.
+ * @NFC_DIGITAL_DRV_CAPS_TG_CRC: The driver handles CRC calculation in target
+ *      mode.
+ */
+#define NFC_DIGITAL_DRV_CAPS_IN_CRC     0x0001
+#define NFC_DIGITAL_DRV_CAPS_TG_CRC     0x0002
+
+struct nfc_digital_dev {
+        struct nfc_dev *nfc_dev;
+        struct nfc_digital_ops *ops;
+
+        u32 protocols;
+
+        int tx_headroom;
+        int tx_tailroom;
+
+        u32 driver_capabilities;
+        void *driver_data;
+
+        struct digital_poll_tech poll_techs[NFC_DIGITAL_POLL_MODE_COUNT_MAX];
+        u8 poll_tech_count;
+        u8 poll_tech_index;
+        struct mutex poll_lock;
+
+        struct work_struct cmd_work;
+        struct work_struct cmd_complete_work;
+        struct list_head cmd_queue;
+        struct mutex cmd_lock;
+
+        struct work_struct poll_work;
+
+        u8 curr_protocol;
+        u8 curr_rf_tech;
+        u8 curr_nfc_dep_pni;
+
+        int (*skb_check_crc)(struct sk_buff *skb);
+        void (*skb_add_crc)(struct sk_buff *skb);
+};
+
+struct nfc_digital_dev *nfc_digital_allocate_device(struct nfc_digital_ops *ops,
+                                                    __u32 supported_protocols,
+                                                    __u32 driver_capabilities,
+                                                    int tx_headroom,
+                                                    int tx_tailroom);
+void nfc_digital_free_device(struct nfc_digital_dev *ndev);
+int nfc_digital_register_device(struct nfc_digital_dev *ndev);
+void nfc_digital_unregister_device(struct nfc_digital_dev *ndev);
+
+static inline void nfc_digital_set_parent_dev(struct nfc_digital_dev *ndev,
+                                              struct device *dev)
+{
+        nfc_set_parent_dev(ndev->nfc_dev, dev);
+}
+
+static inline void nfc_digital_set_drvdata(struct nfc_digital_dev *dev,
+                                           void *data)
+{
+        dev->driver_data = data;
+}
+
+static inline void *nfc_digital_get_drvdata(struct nfc_digital_dev *dev)
+{
+        return dev->driver_data;
+}
+
+#endif /* __NFC_DIGITAL_H */
diff --git a/include/net/nfc/hci.h b/include/net/nfc/hci.h
index b64b7bce4b94..2eca2960ca9c 100644
--- a/include/net/nfc/hci.h
+++ b/include/net/nfc/hci.h
@@ -24,12 +24,6 @@
 
 #include <net/nfc/nfc.h>
 
-struct nfc_phy_ops {
-        int (*write)(void *dev_id, struct sk_buff *skb);
-        int (*enable)(void *dev_id);
-        void (*disable)(void *dev_id);
-};
-
 struct nfc_hci_dev;
 
 struct nfc_hci_ops {
diff --git a/include/net/nfc/nci.h b/include/net/nfc/nci.h
index 88785e5c6b2c..e5aa5acafea0 100644
--- a/include/net/nfc/nci.h
+++ b/include/net/nfc/nci.h
@@ -166,6 +166,10 @@
 #define NCI_GID_NFCEE_MGMT                                      0x2
 #define NCI_GID_PROPRIETARY                                     0xf
 
+/* ----- NCI over SPI head/crc(tail) room needed for outgoing frames ----- */
+#define NCI_SPI_HDR_LEN                                         4
+#define NCI_SPI_CRC_LEN                                         2
+
 /* ---- NCI Packet structures ---- */
 #define NCI_CTRL_HDR_SIZE                                       3
 #define NCI_DATA_HDR_SIZE                                       3
diff --git a/include/net/nfc/nci_core.h b/include/net/nfc/nci_core.h
index 99fc1f3a392a..6126f1f992b4 100644
--- a/include/net/nfc/nci_core.h
+++ b/include/net/nfc/nci_core.h
@@ -207,19 +207,9 @@ int nci_to_errno(__u8 code);
 #define NCI_SPI_CRC_ENABLED     0x01
 
 /* ----- NCI SPI structures ----- */
-struct nci_spi_dev;
-
-struct nci_spi_ops {
-        int (*open)(struct nci_spi_dev *ndev);
-        int (*close)(struct nci_spi_dev *ndev);
-        void (*assert_int)(struct nci_spi_dev *ndev);
-        void (*deassert_int)(struct nci_spi_dev *ndev);
-};
-
-struct nci_spi_dev {
-        struct nci_dev          *nci_dev;
+struct nci_spi {
+        struct nci_dev          *ndev;
         struct spi_device       *spi;
-        struct nci_spi_ops      *ops;
 
         unsigned int            xfer_udelay;    /* microseconds delay between
                                                   transactions */
@@ -227,31 +217,15 @@ struct nci_spi_dev {
 
         struct completion       req_completion;
         u8                      req_result;
-
-        void                    *driver_data;
 };
 
-/* ----- NCI SPI Devices ----- */
-struct nci_spi_dev *nci_spi_allocate_device(struct spi_device *spi,
-                                                struct nci_spi_ops *ops,
-                                                u32 supported_protocols,
-                                                u32 supported_se,
-                                                u8 acknowledge_mode,
-                                                unsigned int delay);
-void nci_spi_free_device(struct nci_spi_dev *ndev);
-int nci_spi_register_device(struct nci_spi_dev *ndev);
-void nci_spi_unregister_device(struct nci_spi_dev *ndev);
-int nci_spi_recv_frame(struct nci_spi_dev *ndev);
-
-static inline void nci_spi_set_drvdata(struct nci_spi_dev *ndev,
-                                            void *data)
-{
-        ndev->driver_data = data;
-}
-
-static inline void *nci_spi_get_drvdata(struct nci_spi_dev *ndev)
-{
-        return ndev->driver_data;
-}
+/* ----- NCI SPI ----- */
+struct nci_spi *nci_spi_allocate_spi(struct spi_device *spi,
+                                     u8 acknowledge_mode, unsigned int delay,
+                                     struct nci_dev *ndev);
+int nci_spi_send(struct nci_spi *nspi,
+                 struct completion *write_handshake_completion,
+                 struct sk_buff *skb);
+struct sk_buff *nci_spi_read(struct nci_spi *nspi);
 
 #endif /* __NCI_CORE_H */
diff --git a/include/net/nfc/nfc.h b/include/net/nfc/nfc.h
index f68ee68e4e3e..82fc4e43fc6e 100644
--- a/include/net/nfc/nfc.h
+++ b/include/net/nfc/nfc.h
@@ -28,9 +28,14 @@
 #include <linux/device.h>
 #include <linux/skbuff.h>
 
-#define nfc_dev_info(dev, fmt, arg...) dev_info((dev), "NFC: " fmt "\n", ## arg)
-#define nfc_dev_err(dev, fmt, arg...) dev_err((dev), "NFC: " fmt "\n", ## arg)
-#define nfc_dev_dbg(dev, fmt, arg...) dev_dbg((dev), fmt "\n", ## arg)
+#define nfc_info(dev, fmt, ...) dev_info((dev), "NFC: " fmt, ##__VA_ARGS__)
+#define nfc_err(dev, fmt, ...) dev_err((dev), "NFC: " fmt, ##__VA_ARGS__)
+
+struct nfc_phy_ops {
+        int (*write)(void *dev_id, struct sk_buff *skb);
+        int (*enable)(void *dev_id);
+        void (*disable)(void *dev_id);
+};
 
 struct nfc_dev;
 
@@ -48,6 +53,8 @@ struct nfc_dev;
 typedef void (*data_exchange_cb_t)(void *context, struct sk_buff *skb,
                                                                 int err);
 
+typedef void (*se_io_cb_t)(void *context, u8 *apdu, size_t apdu_len, int err);
+
 struct nfc_target;
 
 struct nfc_ops {
@@ -74,12 +81,23 @@ struct nfc_ops {
         int (*discover_se)(struct nfc_dev *dev);
         int (*enable_se)(struct nfc_dev *dev, u32 se_idx);
         int (*disable_se)(struct nfc_dev *dev, u32 se_idx);
+        int (*se_io) (struct nfc_dev *dev, u32 se_idx,
+                      u8 *apdu, size_t apdu_length,
+                      se_io_cb_t cb, void *cb_context);
 };
 
 #define NFC_TARGET_IDX_ANY -1
 #define NFC_MAX_GT_LEN 48
 #define NFC_ATR_RES_GT_OFFSET 15
 
+/**
+ * struct nfc_target - NFC target descriptiom
+ *
+ * @sens_res: 2 bytes describing the target SENS_RES response, if the target
+ *      is a type A one. The %sens_res most significant byte must be byte 2
+ *      as described by the NFC Forum digital specification (i.e. the platform
+ *      configuration one) while %sens_res least significant byte is byte 1.
+ */
 struct nfc_target {
         u32 idx;
         u32 supported_protocols;
@@ -243,5 +261,6 @@ void nfc_driver_failure(struct nfc_dev *dev, int err);
 
 int nfc_add_se(struct nfc_dev *dev, u32 se_idx, u16 type);
 int nfc_remove_se(struct nfc_dev *dev, u32 se_idx);
+struct nfc_se *nfc_find_se(struct nfc_dev *dev, u32 se_idx);
 
 #endif /* __NET_NFC_H */
diff --git a/include/uapi/linux/nfc.h b/include/uapi/linux/nfc.h
index 29bed72a4ac4..6ad6cc03ccd3 100644
--- a/include/uapi/linux/nfc.h
+++ b/include/uapi/linux/nfc.h
@@ -85,6 +85,7 @@
  *      a specific SE notifies us about the end of a transaction. The parameter
  *      for this event is the application ID (AID).
  * @NFC_CMD_GET_SE: Dump all discovered secure elements from an NFC controller.
+ * @NFC_CMD_SE_IO: Send/Receive APDUs to/from the selected secure element.
  */
 enum nfc_commands {
         NFC_CMD_UNSPEC,
@@ -114,6 +115,7 @@ enum nfc_commands {
         NFC_EVENT_SE_CONNECTIVITY,
         NFC_EVENT_SE_TRANSACTION,
         NFC_CMD_GET_SE,
+        NFC_CMD_SE_IO,
 /* private: internal use only */
         __NFC_CMD_AFTER_LAST
 };
@@ -147,6 +149,7 @@ enum nfc_commands {
  * @NFC_ATTR_SE_INDEX: Secure element index
  * @NFC_ATTR_SE_TYPE: Secure element type (UICC or EMBEDDED)
  * @NFC_ATTR_FIRMWARE_DOWNLOAD_STATUS: Firmware download operation status
+ * @NFC_ATTR_APDU: Secure element APDU
  */
 enum nfc_attrs {
         NFC_ATTR_UNSPEC,
@@ -174,6 +177,7 @@ enum nfc_attrs {
         NFC_ATTR_SE_TYPE,
         NFC_ATTR_SE_AID,
         NFC_ATTR_FIRMWARE_DOWNLOAD_STATUS,
+        NFC_ATTR_SE_APDU,
 /* private: internal use only */
         __NFC_ATTR_AFTER_LAST
 };
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 17f33a62f6db..60ca52819247 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -15,8 +15,9 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
-#include <net/bluetooth/a2mp.h>
-#include <net/bluetooth/amp.h>
+
+#include "a2mp.h"
+#include "amp.h"
 
 /* Global AMP Manager list */
 LIST_HEAD(amp_mgr_list);
@@ -75,33 +76,26 @@ u8 __next_ident(struct amp_mgr *mgr)
         return mgr->ident;
 }
 
-static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
-{
-        cl->id = 0;
-        cl->type = 0;
-        cl->status = 1;
-}
-
 /* hci_dev_list shall be locked */
-static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
+static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl)
 {
-        int i = 0;
         struct hci_dev *hdev;
+        int i = 1;
 
-        __a2mp_cl_bredr(cl);
+        cl[0].id = AMP_ID_BREDR;
+        cl[0].type = AMP_TYPE_BREDR;
+        cl[0].status = AMP_STATUS_BLUETOOTH_ONLY;
 
         list_for_each_entry(hdev, &hci_dev_list, list) {
-                /* Iterate through AMP controllers */
-                if (hdev->id == HCI_BREDR_ID)
-                        continue;
-
-                /* Starting from second entry */
-                if (++i >= num_ctrl)
-                        return;
-
-                cl[i].id = hdev->id;
-                cl[i].type = hdev->amp_type;
-                cl[i].status = hdev->amp_status;
+                if (hdev->dev_type == HCI_AMP) {
+                        cl[i].id = hdev->id;
+                        cl[i].type = hdev->amp_type;
+                        if (test_bit(HCI_UP, &hdev->flags))
+                                cl[i].status = hdev->amp_status;
+                        else
+                                cl[i].status = AMP_STATUS_POWERED_DOWN;
+                        i++;
+                }
         }
 }
 
@@ -129,6 +123,7 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
         struct a2mp_discov_rsp *rsp;
         u16 ext_feat;
         u8 num_ctrl;
+        struct hci_dev *hdev;
 
         if (len < sizeof(*req))
                 return -EINVAL;
@@ -152,7 +147,14 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
         read_lock(&hci_dev_list_lock);
 
-        num_ctrl = __hci_num_ctrl();
+        /* at minimum the BR/EDR needs to be listed */
+        num_ctrl = 1;
+
+        list_for_each_entry(hdev, &hci_dev_list, list) {
+                if (hdev->dev_type == HCI_AMP)
+                        num_ctrl++;
+        }
+
         len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
         rsp = kmalloc(len, GFP_ATOMIC);
         if (!rsp) {
@@ -163,7 +165,7 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
         rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
         rsp->ext_feat = 0;
 
-        __a2mp_add_cl(mgr, rsp->cl, num_ctrl);
+        __a2mp_add_cl(mgr, rsp->cl);
 
         read_unlock(&hci_dev_list_lock);
 
@@ -208,7 +210,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
                 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) {
+                if (cl->id != AMP_ID_BREDR && cl->type != AMP_TYPE_BREDR) {
                         struct a2mp_info_req req;
 
                         found = true;
@@ -344,7 +346,7 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
         tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
 
         hdev = hci_dev_get(req->id);
-        if (!hdev || hdev->amp_type == HCI_BREDR || tmp) {
+        if (!hdev || hdev->amp_type == AMP_TYPE_BREDR || tmp) {
                 struct a2mp_amp_assoc_rsp rsp;
                 rsp.id = req->id;
 
@@ -451,7 +453,7 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
         rsp.remote_id = req->local_id;
 
         hdev = hci_dev_get(req->remote_id);
-        if (!hdev || hdev->amp_type != HCI_AMP) {
+        if (!hdev || hdev->amp_type == AMP_TYPE_BREDR) {
                 rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
                 goto send_rsp;
         }
@@ -535,7 +537,8 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
                 goto send_rsp;
         }
 
-        hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst);
+        hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
+                                       &mgr->l2cap_conn->hcon->dst);
         if (!hcon) {
                 BT_ERR("No phys link exist");
                 rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
@@ -871,7 +874,7 @@ void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
         rsp.id = hdev->id;
         rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 
-        if (hdev->amp_type != HCI_BREDR) {
+        if (hdev->amp_type != AMP_TYPE_BREDR) {
                 rsp.status = 0;
                 rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
                 rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
diff --git a/include/net/bluetooth/a2mp.h b/net/bluetooth/a2mp.h
index 487b54c1308f..487b54c1308f 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/net/bluetooth/a2mp.h
diff --git a/net/bluetooth/af_bluetooth.c b/net/bluetooth/af_bluetooth.c
index e6e1278dca89..1f1a1118f489 100644
--- a/net/bluetooth/af_bluetooth.c
+++ b/net/bluetooth/af_bluetooth.c
@@ -30,7 +30,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <linux/proc_fs.h>
 
-#define VERSION "2.16"
+#define VERSION "2.17"
 
 /* Bluetooth sockets */
 #define BT_MAX_PROTO    8
@@ -221,12 +221,12 @@ int bt_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
         if (flags & (MSG_OOB))
                 return -EOPNOTSUPP;
 
-        msg->msg_namelen = 0;
-
         skb = skb_recv_datagram(sk, flags, noblock, &err);
         if (!skb) {
-                if (sk->sk_shutdown & RCV_SHUTDOWN)
+                if (sk->sk_shutdown & RCV_SHUTDOWN) {
+                        msg->msg_namelen = 0;
                         return 0;
+                }
                 return err;
         }
 
@@ -238,9 +238,16 @@ int bt_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
 
         skb_reset_transport_header(skb);
         err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
-        if (err == 0)
+        if (err == 0) {
                 sock_recv_ts_and_drops(msg, sk, skb);
 
+                if (bt_sk(sk)->skb_msg_name)
+                        bt_sk(sk)->skb_msg_name(skb, msg->msg_name,
+                                                &msg->msg_namelen);
+                else
+                        msg->msg_namelen = 0;
+        }
+
         skb_free_datagram(sk, skb);
 
         return err ? : copied;
@@ -604,7 +611,7 @@ static int bt_seq_show(struct seq_file *seq, void *v)
         struct bt_sock_list *l = s->l;
 
         if (v == SEQ_START_TOKEN) {
-                seq_puts(seq ,"sk               RefCnt Rmem   Wmem   User   Inode  Src Dst Parent");
+                seq_puts(seq ,"sk               RefCnt Rmem   Wmem   User   Inode  Parent");
 
                 if (l->custom_seq_show) {
                         seq_putc(seq, ' ');
@@ -617,15 +624,13 @@ static int bt_seq_show(struct seq_file *seq, void *v)
                 struct bt_sock *bt = bt_sk(sk);
 
                 seq_printf(seq,
-                           "%pK %-6d %-6u %-6u %-6u %-6lu %pMR %pMR %-6lu",
+                           "%pK %-6d %-6u %-6u %-6u %-6lu %-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),
-                           &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
index d459ed43c779..bb39509b3f06 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -14,10 +14,11 @@
 #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>
 
+#include "a2mp.h"
+#include "amp.h"
+
 /* Remote AMP Controllers interface */
 void amp_ctrl_get(struct amp_ctrl *ctrl)
 {
@@ -110,7 +111,7 @@ static u8 __next_handle(struct amp_mgr *mgr)
 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;
+        bdaddr_t *dst = &mgr->l2cap_conn->hcon->dst;
         struct hci_conn *hcon;
 
         hcon = hci_conn_add(hdev, AMP_LINK, dst);
@@ -409,7 +410,8 @@ void amp_create_logical_link(struct l2cap_chan *chan)
         struct hci_cp_create_accept_logical_link cp;
         struct hci_dev *hdev;
 
-        BT_DBG("chan %p hs_hcon %p dst %pMR", chan, hs_hcon, chan->conn->dst);
+        BT_DBG("chan %p hs_hcon %p dst %pMR", chan, hs_hcon,
+               &chan->conn->hcon->dst);
 
         if (!hs_hcon)
                 return;
diff --git a/include/net/bluetooth/amp.h b/net/bluetooth/amp.h
index 7ea3db77ba89..7ea3db77ba89 100644
--- a/include/net/bluetooth/amp.h
+++ b/net/bluetooth/amp.h
diff --git a/net/bluetooth/bnep/core.c b/net/bluetooth/bnep/core.c
index e430b1abcd2f..a841d3e776c5 100644
--- a/net/bluetooth/bnep/core.c
+++ b/net/bluetooth/bnep/core.c
@@ -32,6 +32,7 @@
 #include <asm/unaligned.h>
 
 #include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/hci_core.h>
 
 #include "bnep.h"
@@ -510,20 +511,13 @@ static int bnep_session(void *arg)
 
 static struct device *bnep_get_device(struct bnep_session *session)
 {
-        bdaddr_t *src = &bt_sk(session->sock->sk)->src;
-        bdaddr_t *dst = &bt_sk(session->sock->sk)->dst;
-        struct hci_dev *hdev;
         struct hci_conn *conn;
 
-        hdev = hci_get_route(dst, src);
-        if (!hdev)
+        conn = l2cap_pi(session->sock->sk)->chan->conn->hcon;
+        if (!conn)
                 return NULL;
 
-        conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst);
-
-        hci_dev_put(hdev);
-
-        return conn ? &conn->dev : NULL;
+        return &conn->dev;
 }
 
 static struct device_type bnep_type = {
@@ -539,8 +533,8 @@ int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock)
 
         BT_DBG("");
 
-        baswap((void *) dst, &bt_sk(sock->sk)->dst);
-        baswap((void *) src, &bt_sk(sock->sk)->src);
+        baswap((void *) dst, &l2cap_pi(sock->sk)->chan->dst);
+        baswap((void *) src, &l2cap_pi(sock->sk)->chan->src);
 
         /* session struct allocated as private part of net_device */
         dev = alloc_netdev(sizeof(struct bnep_session),
diff --git a/net/bluetooth/cmtp/core.c b/net/bluetooth/cmtp/core.c
index e0a6ebf2baa6..67fe5e84e68f 100644
--- a/net/bluetooth/cmtp/core.c
+++ b/net/bluetooth/cmtp/core.c
@@ -340,20 +340,20 @@ int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock)
 
         down_write(&cmtp_session_sem);
 
-        s = __cmtp_get_session(&bt_sk(sock->sk)->dst);
+        s = __cmtp_get_session(&l2cap_pi(sock->sk)->chan->dst);
         if (s && s->state == BT_CONNECTED) {
                 err = -EEXIST;
                 goto failed;
         }
 
-        bacpy(&session->bdaddr, &bt_sk(sock->sk)->dst);
+        bacpy(&session->bdaddr, &l2cap_pi(sock->sk)->chan->dst);
 
         session->mtu = min_t(uint, l2cap_pi(sock->sk)->chan->omtu,
                                         l2cap_pi(sock->sk)->chan->imtu);
 
         BT_DBG("mtu %d", session->mtu);
 
-        sprintf(session->name, "%pMR", &bt_sk(sock->sk)->dst);
+        sprintf(session->name, "%pMR", &session->bdaddr);
 
         session->sock  = sock;
         session->state = BT_CONFIG;
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 514148b7a66b..ff04b051792d 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -28,8 +28,9 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
-#include <net/bluetooth/a2mp.h>
-#include <net/bluetooth/smp.h>
+
+#include "smp.h"
+#include "a2mp.h"
 
 struct sco_param {
         u16 pkt_type;
@@ -49,30 +50,6 @@ static const struct sco_param sco_param_wideband[] = {
         { EDR_ESCO_MASK | ESCO_EV3,   0x0008 }, /* T1 */
 };
 
-static void hci_le_create_connection(struct hci_conn *conn)
-{
-        struct hci_dev *hdev = conn->hdev;
-        struct hci_cp_le_create_conn cp;
-
-        conn->state = BT_CONNECT;
-        conn->out = true;
-        conn->link_mode |= HCI_LM_MASTER;
-        conn->sec_level = BT_SECURITY_LOW;
-
-        memset(&cp, 0, sizeof(cp));
-        cp.scan_interval = __constant_cpu_to_le16(0x0060);
-        cp.scan_window = __constant_cpu_to_le16(0x0030);
-        bacpy(&cp.peer_addr, &conn->dst);
-        cp.peer_addr_type = conn->dst_type;
-        cp.conn_interval_min = __constant_cpu_to_le16(0x0028);
-        cp.conn_interval_max = __constant_cpu_to_le16(0x0038);
-        cp.supervision_timeout = __constant_cpu_to_le16(0x002a);
-        cp.min_ce_len = __constant_cpu_to_le16(0x0000);
-        cp.max_ce_len = __constant_cpu_to_le16(0x0000);
-
-        hci_send_cmd(hdev, HCI_OP_LE_CREATE_CONN, sizeof(cp), &cp);
-}
-
 static void hci_le_create_connection_cancel(struct hci_conn *conn)
 {
         hci_send_cmd(conn->hdev, HCI_OP_LE_CREATE_CONN_CANCEL, 0, NULL);
@@ -404,6 +381,7 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
                 return NULL;
 
         bacpy(&conn->dst, dst);
+        bacpy(&conn->src, &hdev->bdaddr);
         conn->hdev  = hdev;
         conn->type  = type;
         conn->mode  = HCI_CM_ACTIVE;
@@ -546,34 +524,128 @@ struct hci_dev *hci_get_route(bdaddr_t *dst, bdaddr_t *src)
 }
 EXPORT_SYMBOL(hci_get_route);
 
+static void create_le_conn_complete(struct hci_dev *hdev, u8 status)
+{
+        struct hci_conn *conn;
+
+        if (status == 0)
+                return;
+
+        BT_ERR("HCI request failed to create LE connection: status 0x%2.2x",
+               status);
+
+        hci_dev_lock(hdev);
+
+        conn = hci_conn_hash_lookup_state(hdev, LE_LINK, BT_CONNECT);
+        if (!conn)
+                goto done;
+
+        conn->state = BT_CLOSED;
+
+        mgmt_connect_failed(hdev, &conn->dst, conn->type, conn->dst_type,
+                            status);
+
+        hci_proto_connect_cfm(conn, status);
+
+        hci_conn_del(conn);
+
+done:
+        hci_dev_unlock(hdev);
+}
+
+static int hci_create_le_conn(struct hci_conn *conn)
+{
+        struct hci_dev *hdev = conn->hdev;
+        struct hci_cp_le_create_conn cp;
+        struct hci_request req;
+        int err;
+
+        hci_req_init(&req, hdev);
+
+        memset(&cp, 0, sizeof(cp));
+        cp.scan_interval = cpu_to_le16(hdev->le_scan_interval);
+        cp.scan_window = cpu_to_le16(hdev->le_scan_window);
+        bacpy(&cp.peer_addr, &conn->dst);
+        cp.peer_addr_type = conn->dst_type;
+        cp.own_address_type = conn->src_type;
+        cp.conn_interval_min = __constant_cpu_to_le16(0x0028);
+        cp.conn_interval_max = __constant_cpu_to_le16(0x0038);
+        cp.supervision_timeout = __constant_cpu_to_le16(0x002a);
+        cp.min_ce_len = __constant_cpu_to_le16(0x0000);
+        cp.max_ce_len = __constant_cpu_to_le16(0x0000);
+        hci_req_add(&req, HCI_OP_LE_CREATE_CONN, sizeof(cp), &cp);
+
+        err = hci_req_run(&req, create_le_conn_complete);
+        if (err) {
+                hci_conn_del(conn);
+                return err;
+        }
+
+        return 0;
+}
+
 static struct hci_conn *hci_connect_le(struct hci_dev *hdev, bdaddr_t *dst,
                                     u8 dst_type, u8 sec_level, u8 auth_type)
 {
-        struct hci_conn *le;
+        struct hci_conn *conn;
+        int err;
 
-        if (test_bit(HCI_LE_PERIPHERAL, &hdev->flags))
+        if (test_bit(HCI_ADVERTISING, &hdev->flags))
                 return ERR_PTR(-ENOTSUPP);
 
-        le = hci_conn_hash_lookup_ba(hdev, LE_LINK, dst);
-        if (!le) {
-                le = hci_conn_hash_lookup_state(hdev, LE_LINK, BT_CONNECT);
-                if (le)
-                        return ERR_PTR(-EBUSY);
+        /* Some devices send ATT messages as soon as the physical link is
+         * established. To be able to handle these ATT messages, the user-
+         * space first establishes the connection and then starts the pairing
+         * process.
+         *
+         * So if a hci_conn object already exists for the following connection
+         * attempt, we simply update pending_sec_level and auth_type fields
+         * and return the object found.
+         */
+        conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, dst);
+        if (conn) {
+                conn->pending_sec_level = sec_level;
+                conn->auth_type = auth_type;
+                goto done;
+        }
 
-                le = hci_conn_add(hdev, LE_LINK, dst);
-                if (!le)
-                        return ERR_PTR(-ENOMEM);
+        /* Since the controller supports only one LE connection attempt at a
+         * time, we return -EBUSY if there is any connection attempt running.
+         */
+        conn = hci_conn_hash_lookup_state(hdev, LE_LINK, BT_CONNECT);
+        if (conn)
+                return ERR_PTR(-EBUSY);
+
+        conn = hci_conn_add(hdev, LE_LINK, dst);
+        if (!conn)
+                return ERR_PTR(-ENOMEM);
 
-                le->dst_type = bdaddr_to_le(dst_type);
-                hci_le_create_connection(le);
+        if (dst_type == BDADDR_LE_PUBLIC)
+                conn->dst_type = ADDR_LE_DEV_PUBLIC;
+        else
+                conn->dst_type = ADDR_LE_DEV_RANDOM;
+
+        if (bacmp(&conn->src, BDADDR_ANY)) {
+                conn->src_type = ADDR_LE_DEV_PUBLIC;
+        } else {
+                bacpy(&conn->src, &hdev->static_addr);
+                conn->src_type = ADDR_LE_DEV_RANDOM;
         }
 
-        le->pending_sec_level = sec_level;
-        le->auth_type = auth_type;
+        conn->state = BT_CONNECT;
+        conn->out = true;
+        conn->link_mode |= HCI_LM_MASTER;
+        conn->sec_level = BT_SECURITY_LOW;
+        conn->pending_sec_level = sec_level;
+        conn->auth_type = auth_type;
 
-        hci_conn_hold(le);
+        err = hci_create_le_conn(conn);
+        if (err)
+                return ERR_PTR(err);
 
-        return le;
+done:
+        hci_conn_hold(conn);
+        return conn;
 }
 
 static struct hci_conn *hci_connect_acl(struct hci_dev *hdev, bdaddr_t *dst,
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 82dbdc6a7e9e..7add9c96e32c 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -307,11 +307,23 @@ static void amp_init(struct hci_request *req)
         /* Read Local Version */
         hci_req_add(req, HCI_OP_READ_LOCAL_VERSION, 0, NULL);
 
+        /* Read Local Supported Commands */
+        hci_req_add(req, HCI_OP_READ_LOCAL_COMMANDS, 0, NULL);
+
+        /* Read Local Supported Features */
+        hci_req_add(req, HCI_OP_READ_LOCAL_FEATURES, 0, NULL);
+
         /* Read Local AMP Info */
         hci_req_add(req, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
 
         /* Read Data Blk size */
         hci_req_add(req, HCI_OP_READ_DATA_BLOCK_SIZE, 0, NULL);
+
+        /* Read Flow Control Mode */
+        hci_req_add(req, HCI_OP_READ_FLOW_CONTROL_MODE, 0, NULL);
+
+        /* Read Location Data */
+        hci_req_add(req, HCI_OP_READ_LOCATION_DATA, 0, NULL);
 }
 
 static void hci_init1_req(struct hci_request *req, unsigned long opt)
@@ -341,6 +353,8 @@ static void hci_init1_req(struct hci_request *req, unsigned long opt)
 
 static void bredr_setup(struct hci_request *req)
 {
+        struct hci_dev *hdev = req->hdev;
+
         __le16 param;
         __u8 flt_type;
 
@@ -356,6 +370,12 @@ static void bredr_setup(struct hci_request *req)
         /* Read Voice Setting */
         hci_req_add(req, HCI_OP_READ_VOICE_SETTING, 0, NULL);
 
+        /* Read Number of Supported IAC */
+        hci_req_add(req, HCI_OP_READ_NUM_SUPPORTED_IAC, 0, NULL);
+
+        /* Read Current IAC LAP */
+        hci_req_add(req, HCI_OP_READ_CURRENT_IAC_LAP, 0, NULL);
+
         /* Clear Event Filters */
         flt_type = HCI_FLT_CLEAR_ALL;
         hci_req_add(req, HCI_OP_SET_EVENT_FLT, 1, &flt_type);
@@ -364,8 +384,10 @@ static void bredr_setup(struct hci_request *req)
         param = __constant_cpu_to_le16(0x7d00);
         hci_req_add(req, HCI_OP_WRITE_CA_TIMEOUT, 2, &param);
 
-        /* Read page scan parameters */
-        if (req->hdev->hci_ver > BLUETOOTH_VER_1_1) {
+        /* AVM Berlin (31), aka "BlueFRITZ!", reports version 1.2,
+         * but it does not support page scan related HCI commands.
+         */
+        if (hdev->manufacturer != 31 && hdev->hci_ver > BLUETOOTH_VER_1_1) {
                 hci_req_add(req, HCI_OP_READ_PAGE_SCAN_ACTIVITY, 0, NULL);
                 hci_req_add(req, HCI_OP_READ_PAGE_SCAN_TYPE, 0, NULL);
         }
@@ -1036,6 +1058,11 @@ int hci_inquiry(void __user *arg)
                 goto done;
         }
 
+        if (hdev->dev_type != HCI_BREDR) {
+                err = -EOPNOTSUPP;
+                goto done;
+        }
+
         if (!test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags)) {
                 err = -EOPNOTSUPP;
                 goto done;
@@ -1105,7 +1132,7 @@ static u8 create_ad(struct hci_dev *hdev, u8 *ptr)
         u8 ad_len = 0, flags = 0;
         size_t name_len;
 
-        if (test_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags))
+        if (test_bit(HCI_ADVERTISING, &hdev->dev_flags))
                 flags |= LE_AD_GENERAL;
 
         if (test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags)) {
@@ -1196,13 +1223,29 @@ static int hci_dev_do_open(struct hci_dev *hdev)
                 goto done;
         }
 
-        /* Check for rfkill but allow the HCI setup stage to proceed
-         * (which in itself doesn't cause any RF activity).
-         */
-        if (test_bit(HCI_RFKILLED, &hdev->dev_flags) &&
-            !test_bit(HCI_SETUP, &hdev->dev_flags)) {
-                ret = -ERFKILL;
-                goto done;
+        if (!test_bit(HCI_SETUP, &hdev->dev_flags)) {
+                /* Check for rfkill but allow the HCI setup stage to
+                 * proceed (which in itself doesn't cause any RF activity).
+                 */
+                if (test_bit(HCI_RFKILLED, &hdev->dev_flags)) {
+                        ret = -ERFKILL;
+                        goto done;
+                }
+
+                /* Check for valid public address or a configured static
+                 * random adddress, but let the HCI setup proceed to
+                 * be able to determine if there is a public address
+                 * or not.
+                 *
+                 * This check is only valid for BR/EDR controllers
+                 * since AMP controllers do not have an address.
+                 */
+                if (hdev->dev_type == HCI_BREDR &&
+                    !bacmp(&hdev->bdaddr, BDADDR_ANY) &&
+                    !bacmp(&hdev->static_addr, BDADDR_ANY)) {
+                        ret = -EADDRNOTAVAIL;
+                        goto done;
+                }
         }
 
         if (test_bit(HCI_UP, &hdev->flags)) {
@@ -1238,7 +1281,7 @@ static int hci_dev_do_open(struct hci_dev *hdev)
                 hci_notify(hdev, HCI_DEV_UP);
                 if (!test_bit(HCI_SETUP, &hdev->dev_flags) &&
                     !test_bit(HCI_USER_CHANNEL, &hdev->dev_flags) &&
-                    mgmt_valid_hdev(hdev)) {
+                    hdev->dev_type == HCI_BREDR) {
                         hci_dev_lock(hdev);
                         mgmt_powered(hdev, 1);
                         hci_dev_unlock(hdev);
@@ -1288,6 +1331,10 @@ int hci_dev_open(__u16 dev)
         if (test_and_clear_bit(HCI_AUTO_OFF, &hdev->dev_flags))
                 cancel_delayed_work(&hdev->power_off);
 
+        /* After this call it is guaranteed that the setup procedure
+         * has finished. This means that error conditions like RFKILL
+         * or no valid public or static random address apply.
+         */
         flush_workqueue(hdev->req_workqueue);
 
         err = hci_dev_do_open(hdev);
@@ -1341,6 +1388,7 @@ static int hci_dev_do_close(struct hci_dev *hdev)
         skb_queue_purge(&hdev->cmd_q);
         atomic_set(&hdev->cmd_cnt, 1);
         if (!test_bit(HCI_RAW, &hdev->flags) &&
+            !test_bit(HCI_AUTO_OFF, &hdev->dev_flags) &&
             test_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks)) {
                 set_bit(HCI_INIT, &hdev->flags);
                 __hci_req_sync(hdev, hci_reset_req, 0, HCI_CMD_TIMEOUT);
@@ -1373,15 +1421,16 @@ static int hci_dev_do_close(struct hci_dev *hdev)
         hdev->flags = 0;
         hdev->dev_flags &= ~HCI_PERSISTENT_MASK;
 
-        if (!test_and_clear_bit(HCI_AUTO_OFF, &hdev->dev_flags) &&
-            mgmt_valid_hdev(hdev)) {
-                hci_dev_lock(hdev);
-                mgmt_powered(hdev, 0);
-                hci_dev_unlock(hdev);
+        if (!test_and_clear_bit(HCI_AUTO_OFF, &hdev->dev_flags)) {
+                if (hdev->dev_type == HCI_BREDR) {
+                        hci_dev_lock(hdev);
+                        mgmt_powered(hdev, 0);
+                        hci_dev_unlock(hdev);
+                }
         }
 
         /* Controller radio is available but is currently powered down */
-        hdev->amp_status = 0;
+        hdev->amp_status = AMP_STATUS_POWERED_DOWN;
 
         memset(hdev->eir, 0, sizeof(hdev->eir));
         memset(hdev->dev_class, 0, sizeof(hdev->dev_class));
@@ -1500,6 +1549,11 @@ int hci_dev_cmd(unsigned int cmd, void __user *arg)
                 goto done;
         }
 
+        if (hdev->dev_type != HCI_BREDR) {
+                err = -EOPNOTSUPP;
+                goto done;
+        }
+
         if (!test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags)) {
                 err = -EOPNOTSUPP;
                 goto done;
@@ -1703,7 +1757,14 @@ static void hci_power_on(struct work_struct *work)
                 return;
         }
 
-        if (test_bit(HCI_RFKILLED, &hdev->dev_flags)) {
+        /* During the HCI setup phase, a few error conditions are
+         * ignored and they need to be checked now. If they are still
+         * valid, it is important to turn the device back off.
+         */
+        if (test_bit(HCI_RFKILLED, &hdev->dev_flags) ||
+            (hdev->dev_type == HCI_BREDR &&
+             !bacmp(&hdev->bdaddr, BDADDR_ANY) &&
+             !bacmp(&hdev->static_addr, BDADDR_ANY))) {
                 clear_bit(HCI_AUTO_OFF, &hdev->dev_flags);
                 hci_dev_do_close(hdev);
         } else if (test_bit(HCI_AUTO_OFF, &hdev->dev_flags)) {
@@ -2216,13 +2277,17 @@ struct hci_dev *hci_alloc_dev(void)
         hdev->pkt_type  = (HCI_DM1 | HCI_DH1 | HCI_HV1);
         hdev->esco_type = (ESCO_HV1);
         hdev->link_mode = (HCI_LM_ACCEPT);
-        hdev->io_capability = 0x03; /* No Input No Output */
+        hdev->num_iac = 0x01;           /* One IAC support is mandatory */
+        hdev->io_capability = 0x03;     /* No Input No Output */
         hdev->inq_tx_power = HCI_TX_POWER_INVALID;
         hdev->adv_tx_power = HCI_TX_POWER_INVALID;
 
         hdev->sniff_max_interval = 800;
         hdev->sniff_min_interval = 80;
 
+        hdev->le_scan_interval = 0x0060;
+        hdev->le_scan_window = 0x0030;
+
         mutex_init(&hdev->lock);
         mutex_init(&hdev->req_lock);
 
@@ -2329,9 +2394,9 @@ int hci_register_dev(struct hci_dev *hdev)
                 set_bit(HCI_RFKILLED, &hdev->dev_flags);
 
         set_bit(HCI_SETUP, &hdev->dev_flags);
+        set_bit(HCI_AUTO_OFF, &hdev->dev_flags);
 
-        if (hdev->dev_type != HCI_AMP) {
-                set_bit(HCI_AUTO_OFF, &hdev->dev_flags);
+        if (hdev->dev_type == HCI_BREDR) {
                 /* Assume BR/EDR support until proven otherwise (such as
                  * through reading supported features during init.
                  */
@@ -2435,9 +2500,8 @@ int hci_resume_dev(struct hci_dev *hdev)
 EXPORT_SYMBOL(hci_resume_dev);
 
 /* Receive frame from HCI drivers */
-int hci_recv_frame(struct sk_buff *skb)
+int hci_recv_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        struct hci_dev *hdev = (struct hci_dev *) skb->dev;
         if (!hdev || (!test_bit(HCI_UP, &hdev->flags)
                       && !test_bit(HCI_INIT, &hdev->flags))) {
                 kfree_skb(skb);
@@ -2496,7 +2560,6 @@ static int hci_reassembly(struct hci_dev *hdev, int type, void *data,
                 scb->expect = hlen;
                 scb->pkt_type = type;
 
-                skb->dev = (void *) hdev;
                 hdev->reassembly[index] = skb;
         }
 
@@ -2556,7 +2619,7 @@ static int hci_reassembly(struct hci_dev *hdev, int type, void *data,
                         /* Complete frame */
 
                         bt_cb(skb)->pkt_type = type;
-                        hci_recv_frame(skb);
+                        hci_recv_frame(hdev, skb);
 
                         hdev->reassembly[index] = NULL;
                         return remain;
@@ -2647,15 +2710,8 @@ int hci_unregister_cb(struct hci_cb *cb)
 }
 EXPORT_SYMBOL(hci_unregister_cb);
 
-static int hci_send_frame(struct sk_buff *skb)
+static void hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        struct hci_dev *hdev = (struct hci_dev *) skb->dev;
-
-        if (!hdev) {
-                kfree_skb(skb);
-                return -ENODEV;
-        }
-
         BT_DBG("%s type %d len %d", hdev->name, bt_cb(skb)->pkt_type, skb->len);
 
         /* Time stamp */
@@ -2672,7 +2728,8 @@ static int hci_send_frame(struct sk_buff *skb)
         /* Get rid of skb owner, prior to sending to the driver. */
         skb_orphan(skb);
 
-        return hdev->send(skb);
+        if (hdev->send(hdev, skb) < 0)
+                BT_ERR("%s sending frame failed", hdev->name);
 }
 
 void hci_req_init(struct hci_request *req, struct hci_dev *hdev)
@@ -2735,7 +2792,6 @@ static struct sk_buff *hci_prepare_cmd(struct hci_dev *hdev, u16 opcode,
         BT_DBG("skb len %d", skb->len);
 
         bt_cb(skb)->pkt_type = HCI_COMMAND_PKT;
-        skb->dev = (void *) hdev;
 
         return skb;
 }
@@ -2879,7 +2935,6 @@ static void hci_queue_acl(struct hci_chan *chan, struct sk_buff_head *queue,
                 do {
                         skb = list; list = list->next;
 
-                        skb->dev = (void *) hdev;
                         bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT;
                         hci_add_acl_hdr(skb, conn->handle, flags);
 
@@ -2898,8 +2953,6 @@ void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags)
 
         BT_DBG("%s chan %p flags 0x%4.4x", hdev->name, chan, flags);
 
-        skb->dev = (void *) hdev;
-
         hci_queue_acl(chan, &chan->data_q, skb, flags);
 
         queue_work(hdev->workqueue, &hdev->tx_work);
@@ -2920,7 +2973,6 @@ void hci_send_sco(struct hci_conn *conn, struct sk_buff *skb)
         skb_reset_transport_header(skb);
         memcpy(skb_transport_header(skb), &hdr, HCI_SCO_HDR_SIZE);
 
-        skb->dev = (void *) hdev;
         bt_cb(skb)->pkt_type = HCI_SCODATA_PKT;
 
         skb_queue_tail(&conn->data_q, skb);
@@ -3185,7 +3237,7 @@ static void hci_sched_acl_pkt(struct hci_dev *hdev)
                         hci_conn_enter_active_mode(chan->conn,
                                                    bt_cb(skb)->force_active);
 
-                        hci_send_frame(skb);
+                        hci_send_frame(hdev, skb);
                         hdev->acl_last_tx = jiffies;
 
                         hdev->acl_cnt--;
@@ -3237,7 +3289,7 @@ static void hci_sched_acl_blk(struct hci_dev *hdev)
                         hci_conn_enter_active_mode(chan->conn,
                                                    bt_cb(skb)->force_active);
 
-                        hci_send_frame(skb);
+                        hci_send_frame(hdev, skb);
                         hdev->acl_last_tx = jiffies;
 
                         hdev->block_cnt -= blocks;
@@ -3290,7 +3342,7 @@ static void hci_sched_sco(struct hci_dev *hdev)
         while (hdev->sco_cnt && (conn = hci_low_sent(hdev, SCO_LINK, &quote))) {
                 while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
                         BT_DBG("skb %p len %d", skb, skb->len);
-                        hci_send_frame(skb);
+                        hci_send_frame(hdev, skb);
 
                         conn->sent++;
                         if (conn->sent == ~0)
@@ -3314,7 +3366,7 @@ static void hci_sched_esco(struct hci_dev *hdev)
                                                      &quote))) {
                 while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
                         BT_DBG("skb %p len %d", skb, skb->len);
-                        hci_send_frame(skb);
+                        hci_send_frame(hdev, skb);
 
                         conn->sent++;
                         if (conn->sent == ~0)
@@ -3356,7 +3408,7 @@ static void hci_sched_le(struct hci_dev *hdev)
 
                         skb = skb_dequeue(&chan->data_q);
 
-                        hci_send_frame(skb);
+                        hci_send_frame(hdev, skb);
                         hdev->le_last_tx = jiffies;
 
                         cnt--;
@@ -3392,7 +3444,7 @@ static void hci_tx_work(struct work_struct *work)
 
         /* Send next queued raw (unknown type) packet */
         while ((skb = skb_dequeue(&hdev->raw_q)))
-                hci_send_frame(skb);
+                hci_send_frame(hdev, skb);
 }
 
 /* ----- HCI RX task (incoming data processing) ----- */
@@ -3638,7 +3690,7 @@ static void hci_cmd_work(struct work_struct *work)
                 hdev->sent_cmd = skb_clone(skb, GFP_KERNEL);
                 if (hdev->sent_cmd) {
                         atomic_dec(&hdev->cmd_cnt);
-                        hci_send_frame(skb);
+                        hci_send_frame(hdev, skb);
                         if (test_bit(HCI_RESET, &hdev->flags))
                                 del_timer(&hdev->cmd_timer);
                         else
@@ -3650,15 +3702,3 @@ static void hci_cmd_work(struct work_struct *work)
                 }
         }
 }
-
-u8 bdaddr_to_le(u8 bdaddr_type)
-{
-        switch (bdaddr_type) {
-        case BDADDR_LE_PUBLIC:
-                return ADDR_LE_DEV_PUBLIC;
-
-        default:
-                /* Fallback to LE Random address type */
-                return ADDR_LE_DEV_RANDOM;
-        }
-}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 4785ab0795f5..5391469ff1a5 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -29,8 +29,9 @@
 #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>
+
+#include "a2mp.h"
+#include "amp.h"
 
 /* Handle HCI Event packets */
 
@@ -417,6 +418,21 @@ static void hci_cc_write_voice_setting(struct hci_dev *hdev,
                 hdev->notify(hdev, HCI_NOTIFY_VOICE_SETTING);
 }
 
+static void hci_cc_read_num_supported_iac(struct hci_dev *hdev,
+                                          struct sk_buff *skb)
+{
+        struct hci_rp_read_num_supported_iac *rp = (void *) skb->data;
+
+        BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+        if (rp->status)
+                return;
+
+        hdev->num_iac = rp->num_iac;
+
+        BT_DBG("%s num iac %d", hdev->name, hdev->num_iac);
+}
+
 static void hci_cc_write_ssp_mode(struct hci_dev *hdev, struct sk_buff *skb)
 {
         __u8 status = *((__u8 *) skb->data);
@@ -918,12 +934,12 @@ static void hci_cc_le_set_adv_enable(struct hci_dev *hdev, struct sk_buff *skb)
 
         if (!status) {
                 if (*sent)
-                        set_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags);
+                        set_bit(HCI_ADVERTISING, &hdev->dev_flags);
                 else
-                        clear_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags);
+                        clear_bit(HCI_ADVERTISING, &hdev->dev_flags);
         }
 
-        if (!test_bit(HCI_INIT, &hdev->flags)) {
+        if (*sent && !test_bit(HCI_INIT, &hdev->flags)) {
                 struct hci_request req;
 
                 hci_req_init(&req, hdev);
@@ -1005,7 +1021,7 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
                 } else {
                         hdev->features[1][0] &= ~LMP_HOST_LE;
                         clear_bit(HCI_LE_ENABLED, &hdev->dev_flags);
-                        clear_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags);
+                        clear_bit(HCI_ADVERTISING, &hdev->dev_flags);
                 }
 
                 if (sent->simul)
@@ -1296,9 +1312,11 @@ static void hci_cs_remote_name_req(struct hci_dev *hdev, __u8 status)
                 goto unlock;
 
         if (!test_and_set_bit(HCI_CONN_AUTH_PEND, &conn->flags)) {
-                struct hci_cp_auth_requested cp;
-                cp.handle = __cpu_to_le16(conn->handle);
-                hci_send_cmd(hdev, HCI_OP_AUTH_REQUESTED, sizeof(cp), &cp);
+                struct hci_cp_auth_requested auth_cp;
+
+                auth_cp.handle = __cpu_to_le16(conn->handle);
+                hci_send_cmd(hdev, HCI_OP_AUTH_REQUESTED,
+                             sizeof(auth_cp), &auth_cp);
         }
 
 unlock:
@@ -1470,33 +1488,6 @@ static void hci_cs_disconnect(struct hci_dev *hdev, u8 status)
         hci_dev_unlock(hdev);
 }
 
-static void hci_cs_le_create_conn(struct hci_dev *hdev, __u8 status)
-{
-        struct hci_conn *conn;
-
-        BT_DBG("%s status 0x%2.2x", hdev->name, status);
-
-        if (status) {
-                hci_dev_lock(hdev);
-
-                conn = hci_conn_hash_lookup_state(hdev, LE_LINK, BT_CONNECT);
-                if (!conn) {
-                        hci_dev_unlock(hdev);
-                        return;
-                }
-
-                BT_DBG("%s bdaddr %pMR conn %p", hdev->name, &conn->dst, conn);
-
-                conn->state = BT_CLOSED;
-                mgmt_connect_failed(hdev, &conn->dst, conn->type,
-                                    conn->dst_type, status);
-                hci_proto_connect_cfm(conn, status);
-                hci_conn_del(conn);
-
-                hci_dev_unlock(hdev);
-        }
-}
-
 static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
 {
         struct hci_cp_create_phy_link *cp;
@@ -1826,10 +1817,25 @@ static void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
         }
 
         if (ev->status == 0) {
-                if (conn->type == ACL_LINK && conn->flush_key)
+                u8 type = conn->type;
+
+                if (type == ACL_LINK && conn->flush_key)
                         hci_remove_link_key(hdev, &conn->dst);
                 hci_proto_disconn_cfm(conn, ev->reason);
                 hci_conn_del(conn);
+
+                /* Re-enable advertising if necessary, since it might
+                 * have been disabled by the connection. From the
+                 * HCI_LE_Set_Advertise_Enable command description in
+                 * the core specification (v4.0):
+                 * "The Controller shall continue advertising until the Host
+                 * issues an LE_Set_Advertise_Enable command with
+                 * Advertising_Enable set to 0x00 (Advertising is disabled)
+                 * or until a connection is created or until the Advertising
+                 * is timed out due to Directed Advertising."
+                 */
+                if (type == LE_LINK)
+                        mgmt_reenable_advertising(hdev);
         }
 
 unlock:
@@ -2144,6 +2150,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cc_write_voice_setting(hdev, skb);
                 break;
 
+        case HCI_OP_READ_NUM_SUPPORTED_IAC:
+                hci_cc_read_num_supported_iac(hdev, skb);
+                break;
+
         case HCI_OP_WRITE_SSP_MODE:
                 hci_cc_write_ssp_mode(hdev, skb);
                 break;
@@ -2347,10 +2357,6 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_cs_disconnect(hdev, ev->status);
                 break;
 
-        case HCI_OP_LE_CREATE_CONN:
-                hci_cs_le_create_conn(hdev, ev->status);
-                break;
-
         case HCI_OP_CREATE_PHY_LINK:
                 hci_cs_create_phylink(hdev, ev->status);
                 break;
@@ -3490,6 +3496,17 @@ static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 
                 conn->dst_type = ev->bdaddr_type;
 
+                /* The advertising parameters for own address type
+                 * define which source address and source address
+                 * type this connections has.
+                 */
+                if (bacmp(&conn->src, BDADDR_ANY)) {
+                        conn->src_type = ADDR_LE_DEV_PUBLIC;
+                } else {
+                        bacpy(&conn->src, &hdev->static_addr);
+                        conn->src_type = ADDR_LE_DEV_RANDOM;
+                }
+
                 if (ev->role == LE_CONN_ROLE_MASTER) {
                         conn->out = true;
                         conn->link_mode |= HCI_LM_MASTER;
@@ -3645,8 +3662,8 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
         skb_pull(skb, HCI_EVENT_HDR_SIZE);
 
         if (hdev->sent_cmd && bt_cb(hdev->sent_cmd)->req.event == event) {
-                struct hci_command_hdr *hdr = (void *) hdev->sent_cmd->data;
-                u16 opcode = __le16_to_cpu(hdr->opcode);
+                struct hci_command_hdr *cmd_hdr = (void *) hdev->sent_cmd->data;
+                u16 opcode = __le16_to_cpu(cmd_hdr->opcode);
 
                 hci_req_cmd_complete(hdev, opcode, 0);
         }
diff --git a/net/bluetooth/hci_sock.c b/net/bluetooth/hci_sock.c
index 579886186c3a..97f96ebdd56d 100644
--- a/net/bluetooth/hci_sock.c
+++ b/net/bluetooth/hci_sock.c
@@ -387,7 +387,6 @@ static void hci_si_event(struct hci_dev *hdev, int type, int dlen, void *data)
         __net_timestamp(skb);
 
         bt_cb(skb)->pkt_type = HCI_EVENT_PKT;
-        skb->dev = (void *) hdev;
         hci_send_to_sock(hdev, skb);
         kfree_skb(skb);
 }
@@ -518,6 +517,9 @@ static int hci_sock_bound_ioctl(struct sock *sk, unsigned int cmd,
         if (test_bit(HCI_USER_CHANNEL, &hdev->dev_flags))
                 return -EBUSY;
 
+        if (hdev->dev_type != HCI_BREDR)
+                return -EOPNOTSUPP;
+
         switch (cmd) {
         case HCISETRAW:
                 if (!capable(CAP_NET_ADMIN))
@@ -550,10 +552,7 @@ static int hci_sock_bound_ioctl(struct sock *sk, unsigned int cmd,
                 return hci_sock_blacklist_del(hdev, (void __user *) arg);
         }
 
-        if (hdev->ioctl)
-                return hdev->ioctl(hdev, cmd, arg);
-
-        return -EINVAL;
+        return -ENOIOCTLCMD;
 }
 
 static int hci_sock_ioctl(struct socket *sock, unsigned int cmd,
@@ -942,7 +941,6 @@ static int hci_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
 
         bt_cb(skb)->pkt_type = *((unsigned char *) skb->data);
         skb_pull(skb, 1);
-        skb->dev = (void *) hdev;
 
         if (hci_pi(sk)->channel == HCI_CHANNEL_RAW &&
             bt_cb(skb)->pkt_type == HCI_COMMAND_PKT) {
diff --git a/net/bluetooth/hidp/core.c b/net/bluetooth/hidp/core.c
index bdc35a7a7fee..292e619db896 100644
--- a/net/bluetooth/hidp/core.c
+++ b/net/bluetooth/hidp/core.c
@@ -767,10 +767,10 @@ static int hidp_setup_hid(struct hidp_session *session,
         strncpy(hid->name, req->name, sizeof(req->name) - 1);
 
         snprintf(hid->phys, sizeof(hid->phys), "%pMR",
-                 &bt_sk(session->ctrl_sock->sk)->src);
+                 &l2cap_pi(session->ctrl_sock->sk)->chan->src);
 
         snprintf(hid->uniq, sizeof(hid->uniq), "%pMR",
-                 &bt_sk(session->ctrl_sock->sk)->dst);
+                 &l2cap_pi(session->ctrl_sock->sk)->chan->dst);
 
         hid->dev.parent = &session->conn->hcon->dev;
         hid->ll_driver = &hidp_hid_driver;
@@ -1283,23 +1283,29 @@ static int hidp_session_thread(void *arg)
 static int hidp_verify_sockets(struct socket *ctrl_sock,
                                struct socket *intr_sock)
 {
+        struct l2cap_chan *ctrl_chan, *intr_chan;
         struct bt_sock *ctrl, *intr;
         struct hidp_session *session;
 
         if (!l2cap_is_socket(ctrl_sock) || !l2cap_is_socket(intr_sock))
                 return -EINVAL;
 
+        ctrl_chan = l2cap_pi(ctrl_sock->sk)->chan;
+        intr_chan = l2cap_pi(intr_sock->sk)->chan;
+
+        if (bacmp(&ctrl_chan->src, &intr_chan->src) ||
+            bacmp(&ctrl_chan->dst, &intr_chan->dst))
+                return -ENOTUNIQ;
+
         ctrl = bt_sk(ctrl_sock->sk);
         intr = bt_sk(intr_sock->sk);
 
-        if (bacmp(&ctrl->src, &intr->src) || bacmp(&ctrl->dst, &intr->dst))
-                return -ENOTUNIQ;
         if (ctrl->sk.sk_state != BT_CONNECTED ||
             intr->sk.sk_state != BT_CONNECTED)
                 return -EBADFD;
 
         /* early session check, we check again during session registration */
-        session = hidp_session_find(&ctrl->dst);
+        session = hidp_session_find(&ctrl_chan->dst);
         if (session) {
                 hidp_session_put(session);
                 return -EEXIST;
@@ -1332,7 +1338,7 @@ int hidp_connection_add(struct hidp_connadd_req *req,
         if (!conn)
                 return -EBADFD;
 
-        ret = hidp_session_new(&session, &bt_sk(ctrl_sock->sk)->dst, ctrl_sock,
+        ret = hidp_session_new(&session, &chan->dst, ctrl_sock,
                                intr_sock, req, conn);
         if (ret)
                 goto out_conn;
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 02dba4e6df96..0c3446da1ec9 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -36,14 +36,15 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
-#include <net/bluetooth/smp.h>
-#include <net/bluetooth/a2mp.h>
-#include <net/bluetooth/amp.h>
+
+#include "smp.h"
+#include "a2mp.h"
+#include "amp.h"
 
 bool disable_ertm;
 
-static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
-static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP, };
+static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN | L2CAP_FEAT_UCD;
+static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP | L2CAP_FC_CONNLESS, };
 
 static LIST_HEAD(chan_list);
 static DEFINE_RWLOCK(chan_list_lock);
@@ -58,6 +59,18 @@ static void l2cap_send_disconn_req(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);
 
+static inline __u8 bdaddr_type(struct hci_conn *hcon, __u8 type)
+{
+        if (hcon->type == LE_LINK) {
+                if (type == ADDR_LE_DEV_PUBLIC)
+                        return BDADDR_LE_PUBLIC;
+                else
+                        return BDADDR_LE_RANDOM;
+        }
+
+        return BDADDR_BREDR;
+}
+
 /* ---- L2CAP channels ---- */
 
 static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn,
@@ -148,7 +161,7 @@ static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src)
         struct l2cap_chan *c;
 
         list_for_each_entry(c, &chan_list, global_l) {
-                if (c->sport == psm && !bacmp(&bt_sk(c->sk)->src, src))
+                if (c->sport == psm && !bacmp(&c->src, src))
                         return c;
         }
         return NULL;
@@ -620,10 +633,8 @@ void l2cap_chan_del(struct l2cap_chan *chan, int err)
 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", chan, state_to_string(chan->state));
 
         switch (chan->state) {
         case BT_LISTEN:
@@ -634,6 +645,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
         case BT_CONFIG:
                 if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED &&
                     conn->hcon->type == ACL_LINK) {
+                        struct sock *sk = chan->sk;
                         __set_chan_timer(chan, sk->sk_sndtimeo);
                         l2cap_send_disconn_req(chan, reason);
                 } else
@@ -646,10 +658,11 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
                         struct l2cap_conn_rsp rsp;
                         __u16 result;
 
-                        if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags))
+                        if (test_bit(FLAG_DEFER_SETUP, &chan->flags))
                                 result = L2CAP_CR_SEC_BLOCK;
                         else
                                 result = L2CAP_CR_BAD_PSM;
+
                         l2cap_state_change(chan, BT_DISCONN);
 
                         rsp.scid   = cpu_to_le16(chan->dcid);
@@ -676,7 +689,8 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 
 static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
 {
-        if (chan->chan_type == L2CAP_CHAN_RAW) {
+        switch (chan->chan_type) {
+        case L2CAP_CHAN_RAW:
                 switch (chan->sec_level) {
                 case BT_SECURITY_HIGH:
                         return HCI_AT_DEDICATED_BONDING_MITM;
@@ -685,15 +699,29 @@ static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
                 default:
                         return HCI_AT_NO_BONDING;
                 }
-        } else if (chan->psm == __constant_cpu_to_le16(L2CAP_PSM_SDP)) {
-                if (chan->sec_level == BT_SECURITY_LOW)
-                        chan->sec_level = BT_SECURITY_SDP;
-
+                break;
+        case L2CAP_CHAN_CONN_LESS:
+                if (chan->psm == __constant_cpu_to_le16(L2CAP_PSM_3DSP)) {
+                        if (chan->sec_level == BT_SECURITY_LOW)
+                                chan->sec_level = BT_SECURITY_SDP;
+                }
                 if (chan->sec_level == BT_SECURITY_HIGH)
                         return HCI_AT_NO_BONDING_MITM;
                 else
                         return HCI_AT_NO_BONDING;
-        } else {
+                break;
+        case L2CAP_CHAN_CONN_ORIENTED:
+                if (chan->psm == __constant_cpu_to_le16(L2CAP_PSM_SDP)) {
+                        if (chan->sec_level == BT_SECURITY_LOW)
+                                chan->sec_level = BT_SECURITY_SDP;
+
+                        if (chan->sec_level == BT_SECURITY_HIGH)
+                                return HCI_AT_NO_BONDING_MITM;
+                        else
+                                return HCI_AT_NO_BONDING;
+                }
+                /* fall through */
+        default:
                 switch (chan->sec_level) {
                 case BT_SECURITY_HIGH:
                         return HCI_AT_GENERAL_BONDING_MITM;
@@ -702,6 +730,7 @@ static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan)
                 default:
                         return HCI_AT_NO_BONDING;
                 }
+                break;
         }
 }
 
@@ -1015,11 +1044,27 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 static bool __amp_capable(struct l2cap_chan *chan)
 {
         struct l2cap_conn *conn = chan->conn;
+        struct hci_dev *hdev;
+        bool amp_available = false;
+
+        if (!conn->hs_enabled)
+                return false;
 
-        if (conn->hs_enabled && hci_amp_capable() &&
-            chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
-            conn->fixed_chan_mask & L2CAP_FC_A2MP)
-                return true;
+        if (!(conn->fixed_chan_mask & L2CAP_FC_A2MP))
+                return false;
+
+        read_lock(&hci_dev_list_lock);
+        list_for_each_entry(hdev, &hci_dev_list, list) {
+                if (hdev->amp_type != AMP_TYPE_BREDR &&
+                    test_bit(HCI_UP, &hdev->flags)) {
+                        amp_available = true;
+                        break;
+                }
+        }
+        read_unlock(&hci_dev_list_lock);
+
+        if (chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED)
+                return amp_available;
 
         return false;
 }
@@ -1224,8 +1269,6 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
         mutex_lock(&conn->chan_lock);
 
         list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) {
-                struct sock *sk = chan->sk;
-
                 l2cap_chan_lock(chan);
 
                 if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
@@ -1257,9 +1300,10 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
                         rsp.dcid = cpu_to_le16(chan->scid);
 
                         if (l2cap_chan_check_security(chan)) {
+                                struct sock *sk = chan->sk;
+
                                 lock_sock(sk);
-                                if (test_bit(BT_SK_DEFER_SETUP,
-                                             &bt_sk(sk)->flags)) {
+                                if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
                                         rsp.result = __constant_cpu_to_le16(L2CAP_CR_PEND);
                                         rsp.status = __constant_cpu_to_le16(L2CAP_CS_AUTHOR_PEND);
                                         chan->ops->defer(chan);
@@ -1308,8 +1352,6 @@ static struct l2cap_chan *l2cap_global_chan_by_scid(int state, u16 cid,
         read_lock(&chan_list_lock);
 
         list_for_each_entry(c, &chan_list, global_l) {
-                struct sock *sk = c->sk;
-
                 if (state && c->state != state)
                         continue;
 
@@ -1318,16 +1360,16 @@ static struct l2cap_chan *l2cap_global_chan_by_scid(int state, u16 cid,
                         int src_any, dst_any;
 
                         /* Exact match. */
-                        src_match = !bacmp(&bt_sk(sk)->src, src);
-                        dst_match = !bacmp(&bt_sk(sk)->dst, dst);
+                        src_match = !bacmp(&c->src, src);
+                        dst_match = !bacmp(&c->dst, dst);
                         if (src_match && dst_match) {
                                 read_unlock(&chan_list_lock);
                                 return c;
                         }
 
                         /* Closest match */
-                        src_any = !bacmp(&bt_sk(sk)->src, BDADDR_ANY);
-                        dst_any = !bacmp(&bt_sk(sk)->dst, BDADDR_ANY);
+                        src_any = !bacmp(&c->src, BDADDR_ANY);
+                        dst_any = !bacmp(&c->dst, BDADDR_ANY);
                         if ((src_match && dst_any) || (src_any && dst_match) ||
                             (src_any && dst_any))
                                 c1 = c;
@@ -1348,7 +1390,7 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn)
 
         /* Check if we have socket listening on cid */
         pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_ATT,
-                                          conn->src, conn->dst);
+                                          &conn->hcon->src, &conn->hcon->dst);
         if (!pchan)
                 return;
 
@@ -1366,8 +1408,10 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn)
 
         chan->dcid = L2CAP_CID_ATT;
 
-        bacpy(&bt_sk(chan->sk)->src, conn->src);
-        bacpy(&bt_sk(chan->sk)->dst, conn->dst);
+        bacpy(&chan->src, &conn->hcon->src);
+        bacpy(&chan->dst, &conn->hcon->dst);
+        chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
+        chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
 
         __l2cap_chan_add(conn, chan);
 
@@ -1632,9 +1676,6 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon)
                 break;
         }
 
-        conn->src = &hcon->hdev->bdaddr;
-        conn->dst = &hcon->dst;
-
         conn->feat_mask = 0;
 
         if (hcon->type == ACL_LINK)
@@ -1691,8 +1732,6 @@ static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm,
         read_lock(&chan_list_lock);
 
         list_for_each_entry(c, &chan_list, global_l) {
-                struct sock *sk = c->sk;
-
                 if (state && c->state != state)
                         continue;
 
@@ -1701,16 +1740,16 @@ static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm,
                         int src_any, dst_any;
 
                         /* Exact match. */
-                        src_match = !bacmp(&bt_sk(sk)->src, src);
-                        dst_match = !bacmp(&bt_sk(sk)->dst, dst);
+                        src_match = !bacmp(&c->src, src);
+                        dst_match = !bacmp(&c->dst, dst);
                         if (src_match && dst_match) {
                                 read_unlock(&chan_list_lock);
                                 return c;
                         }
 
                         /* Closest match */
-                        src_any = !bacmp(&bt_sk(sk)->src, BDADDR_ANY);
-                        dst_any = !bacmp(&bt_sk(sk)->dst, BDADDR_ANY);
+                        src_any = !bacmp(&c->src, BDADDR_ANY);
+                        dst_any = !bacmp(&c->dst, BDADDR_ANY);
                         if ((src_match && dst_any) || (src_any && dst_match) ||
                             (src_any && dst_any))
                                 c1 = c;
@@ -1726,17 +1765,16 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
                        bdaddr_t *dst, u8 dst_type)
 {
         struct sock *sk = chan->sk;
-        bdaddr_t *src = &bt_sk(sk)->src;
         struct l2cap_conn *conn;
         struct hci_conn *hcon;
         struct hci_dev *hdev;
         __u8 auth_type;
         int err;
 
-        BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", src, dst,
+        BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", &chan->src, dst,
                dst_type, __le16_to_cpu(psm));
 
-        hdev = hci_get_route(dst, src);
+        hdev = hci_get_route(dst, &chan->src);
         if (!hdev)
                 return -EHOSTUNREACH;
 
@@ -1793,9 +1831,8 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
         }
 
         /* Set destination address and psm */
-        lock_sock(sk);
-        bacpy(&bt_sk(sk)->dst, dst);
-        release_sock(sk);
+        bacpy(&chan->dst, dst);
+        chan->dst_type = dst_type;
 
         chan->psm = psm;
         chan->dcid = cid;
@@ -1828,7 +1865,8 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
         }
 
         /* Update source addr of the socket */
-        bacpy(src, conn->src);
+        bacpy(&chan->src, &hcon->src);
+        chan->src_type = bdaddr_type(hcon, hcon->src_type);
 
         l2cap_chan_unlock(chan);
         l2cap_chan_add(conn, chan);
@@ -2266,7 +2304,8 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
         int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE;
         struct l2cap_hdr *lh;
 
-        BT_DBG("chan %p len %zu priority %u", chan, len, priority);
+        BT_DBG("chan %p psm 0x%2.2x len %zu priority %u", chan,
+               __le16_to_cpu(chan->psm), len, priority);
 
         count = min_t(unsigned int, (conn->mtu - hlen), len);
 
@@ -2281,7 +2320,7 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan,
         lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
         lh->cid = cpu_to_le16(chan->dcid);
         lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE);
-        put_unaligned(chan->psm, skb_put(skb, L2CAP_PSMLEN_SIZE));
+        put_unaligned(chan->psm, (__le16 *) skb_put(skb, L2CAP_PSMLEN_SIZE));
 
         err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb);
         if (unlikely(err < 0)) {
@@ -3046,8 +3085,8 @@ int l2cap_ertm_init(struct l2cap_chan *chan)
 
         skb_queue_head_init(&chan->tx_q);
 
-        chan->local_amp_id = 0;
-        chan->move_id = 0;
+        chan->local_amp_id = AMP_ID_BREDR;
+        chan->move_id = AMP_ID_BREDR;
         chan->move_state = L2CAP_MOVE_STABLE;
         chan->move_role = L2CAP_MOVE_ROLE_NONE;
 
@@ -3100,7 +3139,7 @@ static inline bool __l2cap_efs_supported(struct l2cap_conn *conn)
 static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan,
                                       struct l2cap_conf_rfc *rfc)
 {
-        if (chan->local_amp_id && chan->hs_hcon) {
+        if (chan->local_amp_id != AMP_ID_BREDR && chan->hs_hcon) {
                 u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to;
 
                 /* Class 1 devices have must have ERTM timeouts
@@ -3727,7 +3766,8 @@ static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
         BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid);
 
         /* Check if we have socket listening on psm */
-        pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src, conn->dst);
+        pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src,
+                                         &conn->hcon->dst);
         if (!pchan) {
                 result = L2CAP_CR_BAD_PSM;
                 goto sendresp;
@@ -3765,8 +3805,10 @@ static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
          */
         conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT;
 
-        bacpy(&bt_sk(sk)->src, conn->src);
-        bacpy(&bt_sk(sk)->dst, conn->dst);
+        bacpy(&chan->src, &conn->hcon->src);
+        bacpy(&chan->dst, &conn->hcon->dst);
+        chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
+        chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
         chan->psm  = psm;
         chan->dcid = scid;
         chan->local_amp_id = amp_id;
@@ -3781,7 +3823,7 @@ static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
 
         if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
                 if (l2cap_chan_check_security(chan)) {
-                        if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) {
+                        if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
                                 __l2cap_state_change(chan, BT_CONNECT2);
                                 result = L2CAP_CR_PEND;
                                 status = L2CAP_CS_AUTHOR_PEND;
@@ -3791,12 +3833,12 @@ static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
                                  * The connection will succeed after the
                                  * physical link is up.
                                  */
-                                if (amp_id) {
-                                        __l2cap_state_change(chan, BT_CONNECT2);
-                                        result = L2CAP_CR_PEND;
-                                } else {
+                                if (amp_id == AMP_ID_BREDR) {
                                         __l2cap_state_change(chan, BT_CONFIG);
                                         result = L2CAP_CR_SUCCESS;
+                                } else {
+                                        __l2cap_state_change(chan, BT_CONNECT2);
+                                        result = L2CAP_CR_PEND;
                                 }
                                 status = L2CAP_CS_NO_INFO;
                         }
@@ -4423,7 +4465,7 @@ static int l2cap_create_channel_req(struct l2cap_conn *conn,
         BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id);
 
         /* For controller id 0 make BR/EDR connection */
-        if (req->amp_id == HCI_BREDR_ID) {
+        if (req->amp_id == AMP_ID_BREDR) {
                 l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP,
                               req->amp_id);
                 return 0;
@@ -4445,7 +4487,8 @@ static int l2cap_create_channel_req(struct l2cap_conn *conn,
                 struct amp_mgr *mgr = conn->hcon->amp_mgr;
                 struct hci_conn *hs_hcon;
 
-                hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, conn->dst);
+                hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
+                                                  &conn->hcon->dst);
                 if (!hs_hcon) {
                         hci_dev_put(hdev);
                         return -EBADSLT;
@@ -4658,7 +4701,7 @@ void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan,
 
         if (chan->state != BT_CONNECTED) {
                 /* Ignore logical link if channel is on BR/EDR */
-                if (chan->local_amp_id)
+                if (chan->local_amp_id != AMP_ID_BREDR)
                         l2cap_logical_finish_create(chan, hchan);
         } else {
                 l2cap_logical_finish_move(chan, hchan);
@@ -4669,7 +4712,7 @@ void l2cap_move_start(struct l2cap_chan *chan)
 {
         BT_DBG("chan %p", chan);
 
-        if (chan->local_amp_id == HCI_BREDR_ID) {
+        if (chan->local_amp_id == AMP_ID_BREDR) {
                 if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED)
                         return;
                 chan->move_role = L2CAP_MOVE_ROLE_INITIATOR;
@@ -4868,7 +4911,7 @@ static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
                 goto send_move_response;
         }
 
-        if (req->dest_amp_id) {
+        if (req->dest_amp_id != AMP_ID_BREDR) {
                 struct hci_dev *hdev;
                 hdev = hci_dev_get(req->dest_amp_id);
                 if (!hdev || hdev->dev_type != HCI_AMP ||
@@ -4888,7 +4931,7 @@ static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
          */
         if ((__chan_is_moving(chan) ||
              chan->move_role != L2CAP_MOVE_ROLE_NONE) &&
-            bacmp(conn->src, conn->dst) > 0) {
+            bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) {
                 result = L2CAP_MR_COLLISION;
                 goto send_move_response;
         }
@@ -4898,7 +4941,7 @@ static inline int l2cap_move_channel_req(struct l2cap_conn *conn,
         chan->move_id = req->dest_amp_id;
         icid = chan->dcid;
 
-        if (!req->dest_amp_id) {
+        if (req->dest_amp_id == AMP_ID_BREDR) {
                 /* Moving to BR/EDR */
                 if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) {
                         chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY;
@@ -5090,7 +5133,7 @@ static int l2cap_move_channel_confirm(struct l2cap_conn *conn,
         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) {
                 if (result == L2CAP_MC_CONFIRMED) {
                         chan->local_amp_id = chan->move_id;
-                        if (!chan->local_amp_id)
+                        if (chan->local_amp_id == AMP_ID_BREDR)
                                 __release_logical_link(chan);
                 } else {
                         chan->move_id = chan->local_amp_id;
@@ -5130,7 +5173,7 @@ static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn,
         if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) {
                 chan->local_amp_id = chan->move_id;
 
-                if (!chan->local_amp_id && chan->hs_hchan)
+                if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan)
                         __release_logical_link(chan);
 
                 l2cap_move_done(chan);
@@ -6403,7 +6446,8 @@ static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
         if (hcon->type != ACL_LINK)
                 goto drop;
 
-        chan = l2cap_global_chan_by_psm(0, psm, conn->src, conn->dst);
+        chan = l2cap_global_chan_by_psm(0, psm, &conn->hcon->src,
+                                        &conn->hcon->dst);
         if (!chan)
                 goto drop;
 
@@ -6415,6 +6459,10 @@ static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm,
         if (chan->imtu < skb->len)
                 goto drop;
 
+        /* Store remote BD_ADDR and PSM for msg_name */
+        bacpy(&bt_cb(skb)->bdaddr, &conn->hcon->dst);
+        bt_cb(skb)->psm = psm;
+
         if (!chan->ops->recv(chan, skb))
                 return;
 
@@ -6432,7 +6480,7 @@ static void l2cap_att_channel(struct l2cap_conn *conn,
                 goto drop;
 
         chan = l2cap_global_chan_by_scid(BT_CONNECTED, L2CAP_CID_ATT,
-                                         conn->src, conn->dst);
+                                         &conn->hcon->src, &conn->hcon->dst);
         if (!chan)
                 goto drop;
 
@@ -6507,17 +6555,15 @@ int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
         /* Find listening sockets and check their link_mode */
         read_lock(&chan_list_lock);
         list_for_each_entry(c, &chan_list, global_l) {
-                struct sock *sk = c->sk;
-
                 if (c->state != BT_LISTEN)
                         continue;
 
-                if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) {
+                if (!bacmp(&c->src, &hdev->bdaddr)) {
                         lm1 |= HCI_LM_ACCEPT;
                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
                                 lm1 |= HCI_LM_MASTER;
                         exact++;
-                } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
+                } else if (!bacmp(&c->src, BDADDR_ANY)) {
                         lm2 |= HCI_LM_ACCEPT;
                         if (test_bit(FLAG_ROLE_SWITCH, &c->flags))
                                 lm2 |= HCI_LM_MASTER;
@@ -6623,11 +6669,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
                 if (!status && (chan->state == BT_CONNECTED ||
                                 chan->state == BT_CONFIG)) {
-                        struct sock *sk = chan->sk;
-
-                        clear_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags);
-                        sk->sk_state_change(sk);
-
+                        chan->ops->resume(chan);
                         l2cap_check_encryption(chan, encrypt);
                         l2cap_chan_unlock(chan);
                         continue;
@@ -6647,8 +6689,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
                         lock_sock(sk);
 
                         if (!status) {
-                                if (test_bit(BT_SK_DEFER_SETUP,
-                                             &bt_sk(sk)->flags)) {
+                                if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) {
                                         res = L2CAP_CR_PEND;
                                         stat = L2CAP_CS_AUTHOR_PEND;
                                         chan->ops->defer(chan);
@@ -6782,9 +6823,13 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags)
                 conn->rx_len -= skb->len;
 
                 if (!conn->rx_len) {
-                        /* Complete frame received */
-                        l2cap_recv_frame(conn, conn->rx_skb);
+                        /* Complete frame received. l2cap_recv_frame
+                         * takes ownership of the skb so set the global
+                         * rx_skb pointer to NULL first.
+                         */
+                        struct sk_buff *rx_skb = conn->rx_skb;
                         conn->rx_skb = NULL;
+                        l2cap_recv_frame(conn, rx_skb);
                 }
                 break;
         }
@@ -6801,10 +6846,8 @@ static int l2cap_debugfs_show(struct seq_file *f, void *p)
         read_lock(&chan_list_lock);
 
         list_for_each_entry(c, &chan_list, global_l) {
-                struct sock *sk = c->sk;
-
                 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->src, &c->dst,
                            c->state, __le16_to_cpu(c->psm),
                            c->scid, c->dcid, c->imtu, c->omtu,
                            c->sec_level, c->mode);
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index 9119898ef040..5ffd75e20bde 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -32,7 +32,8 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
-#include <net/bluetooth/smp.h>
+
+#include "smp.h"
 
 static struct bt_sock_list l2cap_sk_list = {
         .lock = __RW_LOCK_UNLOCKED(l2cap_sk_list.lock)
@@ -68,6 +69,9 @@ static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
         if (la.l2_cid && la.l2_psm)
                 return -EINVAL;
 
+        if (!bdaddr_type_is_valid(la.l2_bdaddr_type))
+                return -EINVAL;
+
         lock_sock(sk);
 
         if (sk->sk_state != BT_OPEN) {
@@ -99,11 +103,20 @@ static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
         if (err < 0)
                 goto done;
 
-        if (__le16_to_cpu(la.l2_psm) == L2CAP_PSM_SDP ||
-            __le16_to_cpu(la.l2_psm) == L2CAP_PSM_RFCOMM)
-                chan->sec_level = BT_SECURITY_SDP;
+        switch (chan->chan_type) {
+        case L2CAP_CHAN_CONN_LESS:
+                if (__le16_to_cpu(la.l2_psm) == L2CAP_PSM_3DSP)
+                        chan->sec_level = BT_SECURITY_SDP;
+                break;
+        case L2CAP_CHAN_CONN_ORIENTED:
+                if (__le16_to_cpu(la.l2_psm) == L2CAP_PSM_SDP ||
+                    __le16_to_cpu(la.l2_psm) == L2CAP_PSM_RFCOMM)
+                        chan->sec_level = BT_SECURITY_SDP;
+                break;
+        }
 
-        bacpy(&bt_sk(sk)->src, &la.l2_bdaddr);
+        bacpy(&chan->src, &la.l2_bdaddr);
+        chan->src_type = la.l2_bdaddr_type;
 
         chan->state = BT_BOUND;
         sk->sk_state = BT_BOUND;
@@ -134,6 +147,15 @@ static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr,
         if (la.l2_cid && la.l2_psm)
                 return -EINVAL;
 
+        if (!bdaddr_type_is_valid(la.l2_bdaddr_type))
+                return -EINVAL;
+
+        if (chan->src_type == BDADDR_BREDR && la.l2_bdaddr_type != BDADDR_BREDR)
+                return -EINVAL;
+
+        if (chan->src_type != BDADDR_BREDR && la.l2_bdaddr_type == BDADDR_BREDR)
+                return -EINVAL;
+
         err = l2cap_chan_connect(chan, la.l2_psm, __le16_to_cpu(la.l2_cid),
                                  &la.l2_bdaddr, la.l2_bdaddr_type);
         if (err)
@@ -265,12 +287,14 @@ static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr,
 
         if (peer) {
                 la->l2_psm = chan->psm;
-                bacpy(&la->l2_bdaddr, &bt_sk(sk)->dst);
+                bacpy(&la->l2_bdaddr, &chan->dst);
                 la->l2_cid = cpu_to_le16(chan->dcid);
+                la->l2_bdaddr_type = chan->dst_type;
         } else {
                 la->l2_psm = chan->sport;
-                bacpy(&la->l2_bdaddr, &bt_sk(sk)->src);
+                bacpy(&la->l2_bdaddr, &chan->src);
                 la->l2_cid = cpu_to_le16(chan->scid);
+                la->l2_bdaddr_type = chan->src_type;
         }
 
         return 0;
@@ -660,10 +684,13 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname,
                         break;
                 }
 
-                if (opt)
+                if (opt) {
                         set_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags);
-                else
+                        set_bit(FLAG_DEFER_SETUP, &chan->flags);
+                } else {
                         clear_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags);
+                        clear_bit(FLAG_DEFER_SETUP, &chan->flags);
+                }
                 break;
 
         case BT_FLUSHABLE:
@@ -678,7 +705,7 @@ static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname,
                 }
 
                 if (opt == BT_FLUSHABLE_OFF) {
-                        struct l2cap_conn *conn = chan->conn;
+                        conn = chan->conn;
                         /* proceed further only when we have l2cap_conn and
                            No Flush support in the LM */
                         if (!conn || !lmp_no_flush_capable(conn->hcon->hdev)) {
@@ -964,13 +991,12 @@ static struct l2cap_chan *l2cap_sock_new_connection_cb(struct l2cap_chan *chan)
 
 static int l2cap_sock_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 {
-        int err;
         struct sock *sk = chan->data;
-        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        int err;
 
         lock_sock(sk);
 
-        if (pi->rx_busy_skb) {
+        if (l2cap_pi(sk)->rx_busy_skb) {
                 err = -ENOMEM;
                 goto done;
         }
@@ -986,9 +1012,9 @@ static int l2cap_sock_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
          * acked and reassembled until there is buffer space
          * available.
          */
-        if (err < 0 && pi->chan->mode == L2CAP_MODE_ERTM) {
-                pi->rx_busy_skb = skb;
-                l2cap_chan_busy(pi->chan, 1);
+        if (err < 0 && chan->mode == L2CAP_MODE_ERTM) {
+                l2cap_pi(sk)->rx_busy_skb = skb;
+                l2cap_chan_busy(chan, 1);
                 err = 0;
         }
 
@@ -1098,6 +1124,14 @@ static void l2cap_sock_defer_cb(struct l2cap_chan *chan)
                 parent->sk_data_ready(parent, 0);
 }
 
+static void l2cap_sock_resume_cb(struct l2cap_chan *chan)
+{
+        struct sock *sk = chan->data;
+
+        clear_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags);
+        sk->sk_state_change(sk);
+}
+
 static struct l2cap_ops l2cap_chan_ops = {
         .name           = "L2CAP Socket Interface",
         .new_connection = l2cap_sock_new_connection_cb,
@@ -1107,6 +1141,7 @@ static struct l2cap_ops l2cap_chan_ops = {
         .state_change   = l2cap_sock_state_change_cb,
         .ready          = l2cap_sock_ready_cb,
         .defer          = l2cap_sock_defer_cb,
+        .resume         = l2cap_sock_resume_cb,
         .alloc_skb      = l2cap_sock_alloc_skb_cb,
 };
 
@@ -1116,6 +1151,7 @@ static void l2cap_sock_destruct(struct sock *sk)
 
         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;
@@ -1125,10 +1161,22 @@ static void l2cap_sock_destruct(struct sock *sk)
         skb_queue_purge(&sk->sk_write_queue);
 }
 
+static void l2cap_skb_msg_name(struct sk_buff *skb, void *msg_name,
+                               int *msg_namelen)
+{
+        struct sockaddr_l2 *la = (struct sockaddr_l2 *) msg_name;
+
+        memset(la, 0, sizeof(struct sockaddr_l2));
+        la->l2_family = AF_BLUETOOTH;
+        la->l2_psm = bt_cb(skb)->psm;
+        bacpy(&la->l2_bdaddr, &bt_cb(skb)->bdaddr);
+
+        *msg_namelen = sizeof(struct sockaddr_l2);
+}
+
 static void l2cap_sock_init(struct sock *sk, struct sock *parent)
 {
-        struct l2cap_pinfo *pi = l2cap_pi(sk);
-        struct l2cap_chan *chan = pi->chan;
+        struct l2cap_chan *chan = l2cap_pi(sk)->chan;
 
         BT_DBG("sk %p", sk);
 
@@ -1152,13 +1200,13 @@ static void l2cap_sock_init(struct sock *sk, struct sock *parent)
 
                 security_sk_clone(parent, sk);
         } else {
-
                 switch (sk->sk_type) {
                 case SOCK_RAW:
                         chan->chan_type = L2CAP_CHAN_RAW;
                         break;
                 case SOCK_DGRAM:
                         chan->chan_type = L2CAP_CHAN_CONN_LESS;
+                        bt_sk(sk)->skb_msg_name = l2cap_skb_msg_name;
                         break;
                 case SOCK_SEQPACKET:
                 case SOCK_STREAM:
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 16125ff918f1..861e389f4b4c 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -30,7 +30,8 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
-#include <net/bluetooth/smp.h>
+
+#include "smp.h"
 
 #define MGMT_VERSION    1
 #define MGMT_REVISION   4
@@ -77,6 +78,7 @@ static const u16 mgmt_commands[] = {
         MGMT_OP_SET_ADVERTISING,
         MGMT_OP_SET_BREDR,
         MGMT_OP_SET_STATIC_ADDRESS,
+        MGMT_OP_SET_SCAN_PARAMS,
 };
 
 static const u16 mgmt_events[] = {
@@ -182,11 +184,6 @@ static u8 mgmt_status_table[] = {
         MGMT_STATUS_CONNECT_FAILED,     /* MAC Connection Failed */
 };
 
-bool mgmt_valid_hdev(struct hci_dev *hdev)
-{
-        return hdev->dev_type == HCI_BREDR;
-}
-
 static u8 mgmt_status(u8 hci_status)
 {
         if (hci_status < ARRAY_SIZE(mgmt_status_table))
@@ -322,10 +319,8 @@ static int read_index_list(struct sock *sk, struct hci_dev *hdev, void *data,
 
         count = 0;
         list_for_each_entry(d, &hci_dev_list, list) {
-                if (!mgmt_valid_hdev(d))
-                        continue;
-
-                count++;
+                if (d->dev_type == HCI_BREDR)
+                        count++;
         }
 
         rp_len = sizeof(*rp) + (2 * count);
@@ -343,11 +338,10 @@ static int read_index_list(struct sock *sk, struct hci_dev *hdev, void *data,
                 if (test_bit(HCI_USER_CHANNEL, &d->dev_flags))
                         continue;
 
-                if (!mgmt_valid_hdev(d))
-                        continue;
-
-                rp->index[count++] = cpu_to_le16(d->id);
-                BT_DBG("Added hci%u", d->id);
+                if (d->dev_type == HCI_BREDR) {
+                        rp->index[count++] = cpu_to_le16(d->id);
+                        BT_DBG("Added hci%u", d->id);
+                }
         }
 
         rp->num_controllers = cpu_to_le16(count);
@@ -370,9 +364,6 @@ static u32 get_supported_settings(struct hci_dev *hdev)
         settings |= MGMT_SETTING_POWERED;
         settings |= MGMT_SETTING_PAIRABLE;
 
-        if (lmp_ssp_capable(hdev))
-                settings |= MGMT_SETTING_SSP;
-
         if (lmp_bredr_capable(hdev)) {
                 settings |= MGMT_SETTING_CONNECTABLE;
                 if (hdev->hci_ver >= BLUETOOTH_VER_1_2)
@@ -380,7 +371,11 @@ static u32 get_supported_settings(struct hci_dev *hdev)
                 settings |= MGMT_SETTING_DISCOVERABLE;
                 settings |= MGMT_SETTING_BREDR;
                 settings |= MGMT_SETTING_LINK_SECURITY;
-                settings |= MGMT_SETTING_HS;
+
+                if (lmp_ssp_capable(hdev)) {
+                        settings |= MGMT_SETTING_SSP;
+                        settings |= MGMT_SETTING_HS;
+                }
         }
 
         if (lmp_le_capable(hdev)) {
@@ -425,7 +420,7 @@ static u32 get_current_settings(struct hci_dev *hdev)
         if (test_bit(HCI_HS_ENABLED, &hdev->dev_flags))
                 settings |= MGMT_SETTING_HS;
 
-        if (test_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags))
+        if (test_bit(HCI_ADVERTISING, &hdev->dev_flags))
                 settings |= MGMT_SETTING_ADVERTISING;
 
         return settings;
@@ -940,11 +935,52 @@ static u8 mgmt_le_support(struct hci_dev *hdev)
                 return MGMT_STATUS_SUCCESS;
 }
 
+static void set_discoverable_complete(struct hci_dev *hdev, u8 status)
+{
+        struct pending_cmd *cmd;
+        struct mgmt_mode *cp;
+        bool changed;
+
+        BT_DBG("status 0x%02x", status);
+
+        hci_dev_lock(hdev);
+
+        cmd = mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, hdev);
+        if (!cmd)
+                goto unlock;
+
+        if (status) {
+                u8 mgmt_err = mgmt_status(status);
+                cmd_status(cmd->sk, cmd->index, cmd->opcode, mgmt_err);
+                goto remove_cmd;
+        }
+
+        cp = cmd->param;
+        if (cp->val)
+                changed = !test_and_set_bit(HCI_DISCOVERABLE,
+                                            &hdev->dev_flags);
+        else
+                changed = test_and_clear_bit(HCI_DISCOVERABLE,
+                                             &hdev->dev_flags);
+
+        send_settings_rsp(cmd->sk, MGMT_OP_SET_DISCOVERABLE, hdev);
+
+        if (changed)
+                new_settings(hdev, cmd->sk);
+
+remove_cmd:
+        mgmt_pending_remove(cmd);
+
+unlock:
+        hci_dev_unlock(hdev);
+}
+
 static int set_discoverable(struct sock *sk, struct hci_dev *hdev, void *data,
                             u16 len)
 {
         struct mgmt_cp_set_discoverable *cp = data;
         struct pending_cmd *cmd;
+        struct hci_request req;
         u16 timeout;
         u8 scan, status;
         int err;
@@ -1026,6 +1062,8 @@ static int set_discoverable(struct sock *sk, struct hci_dev *hdev, void *data,
                 goto failed;
         }
 
+        hci_req_init(&req, hdev);
+
         scan = SCAN_PAGE;
 
         if (cp->val)
@@ -1033,7 +1071,9 @@ static int set_discoverable(struct sock *sk, struct hci_dev *hdev, void *data,
         else
                 cancel_delayed_work(&hdev->discov_off);
 
-        err = hci_send_cmd(hdev, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
+        hci_req_add(&req, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
+
+        err = hci_req_run(&req, set_discoverable_complete);
         if (err < 0)
                 mgmt_pending_remove(cmd);
 
@@ -1077,9 +1117,58 @@ static void write_fast_connectable(struct hci_request *req, bool enable)
                 hci_req_add(req, HCI_OP_WRITE_PAGE_SCAN_TYPE, 1, &type);
 }
 
+static u8 get_adv_type(struct hci_dev *hdev)
+{
+        struct pending_cmd *cmd;
+        bool connectable;
+
+        /* If there's a pending mgmt command the flag will not yet have
+         * it's final value, so check for this first.
+         */
+        cmd = mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, hdev);
+        if (cmd) {
+                struct mgmt_mode *cp = cmd->param;
+                connectable = !!cp->val;
+        } else {
+                connectable = test_bit(HCI_CONNECTABLE, &hdev->dev_flags);
+        }
+
+        return connectable ? LE_ADV_IND : LE_ADV_NONCONN_IND;
+}
+
+static void enable_advertising(struct hci_request *req)
+{
+        struct hci_dev *hdev = req->hdev;
+        struct hci_cp_le_set_adv_param cp;
+        u8 enable = 0x01;
+
+        memset(&cp, 0, sizeof(cp));
+        cp.min_interval = __constant_cpu_to_le16(0x0800);
+        cp.max_interval = __constant_cpu_to_le16(0x0800);
+        cp.type = get_adv_type(hdev);
+        if (bacmp(&hdev->bdaddr, BDADDR_ANY))
+                cp.own_address_type = ADDR_LE_DEV_PUBLIC;
+        else
+                cp.own_address_type = ADDR_LE_DEV_RANDOM;
+        cp.channel_map = 0x07;
+
+        hci_req_add(req, HCI_OP_LE_SET_ADV_PARAM, sizeof(cp), &cp);
+
+        hci_req_add(req, HCI_OP_LE_SET_ADV_ENABLE, sizeof(enable), &enable);
+}
+
+static void disable_advertising(struct hci_request *req)
+{
+        u8 enable = 0x00;
+
+        hci_req_add(req, HCI_OP_LE_SET_ADV_ENABLE, sizeof(enable), &enable);
+}
+
 static void set_connectable_complete(struct hci_dev *hdev, u8 status)
 {
         struct pending_cmd *cmd;
+        struct mgmt_mode *cp;
+        bool changed;
 
         BT_DBG("status 0x%02x", status);
 
@@ -1089,8 +1178,24 @@ static void set_connectable_complete(struct hci_dev *hdev, u8 status)
         if (!cmd)
                 goto unlock;
 
+        if (status) {
+                u8 mgmt_err = mgmt_status(status);
+                cmd_status(cmd->sk, cmd->index, cmd->opcode, mgmt_err);
+                goto remove_cmd;
+        }
+
+        cp = cmd->param;
+        if (cp->val)
+                changed = !test_and_set_bit(HCI_CONNECTABLE, &hdev->dev_flags);
+        else
+                changed = test_and_clear_bit(HCI_CONNECTABLE, &hdev->dev_flags);
+
         send_settings_rsp(cmd->sk, MGMT_OP_SET_CONNECTABLE, hdev);
 
+        if (changed)
+                new_settings(hdev, cmd->sk);
+
+remove_cmd:
         mgmt_pending_remove(cmd);
 
 unlock:
@@ -1103,15 +1208,15 @@ static int set_connectable(struct sock *sk, struct hci_dev *hdev, void *data,
         struct mgmt_mode *cp = data;
         struct pending_cmd *cmd;
         struct hci_request req;
-        u8 scan, status;
+        u8 scan;
         int err;
 
         BT_DBG("request for %s", hdev->name);
 
-        status = mgmt_bredr_support(hdev);
-        if (status)
+        if (!test_bit(HCI_LE_ENABLED, &hdev->dev_flags) &&
+            !test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags))
                 return cmd_status(sk, hdev->id, MGMT_OP_SET_CONNECTABLE,
-                                  status);
+                                  MGMT_STATUS_REJECTED);
 
         if (cp->val != 0x00 && cp->val != 0x01)
                 return cmd_status(sk, hdev->id, MGMT_OP_SET_CONNECTABLE,
@@ -1149,30 +1254,29 @@ static int set_connectable(struct sock *sk, struct hci_dev *hdev, void *data,
                 goto failed;
         }
 
-        if (!!cp->val == test_bit(HCI_PSCAN, &hdev->flags)) {
-                err = send_settings_rsp(sk, MGMT_OP_SET_CONNECTABLE, hdev);
-                goto failed;
-        }
-
         cmd = mgmt_pending_add(sk, MGMT_OP_SET_CONNECTABLE, hdev, data, len);
         if (!cmd) {
                 err = -ENOMEM;
                 goto failed;
         }
 
-        if (cp->val) {
-                scan = SCAN_PAGE;
-        } else {
-                scan = 0;
+        hci_req_init(&req, hdev);
 
-                if (test_bit(HCI_ISCAN, &hdev->flags) &&
-                    hdev->discov_timeout > 0)
-                        cancel_delayed_work(&hdev->discov_off);
-        }
+        if (test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags) &&
+            cp->val != test_bit(HCI_PSCAN, &hdev->flags)) {
 
-        hci_req_init(&req, hdev);
+                if (cp->val) {
+                        scan = SCAN_PAGE;
+                } else {
+                        scan = 0;
 
-        hci_req_add(&req, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
+                        if (test_bit(HCI_ISCAN, &hdev->flags) &&
+                                        hdev->discov_timeout > 0)
+                                cancel_delayed_work(&hdev->discov_off);
+                }
+
+                hci_req_add(&req, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
+        }
 
         /* If we're going from non-connectable to connectable or
          * vice-versa when fast connectable is enabled ensure that fast
@@ -1183,9 +1287,20 @@ static int set_connectable(struct sock *sk, struct hci_dev *hdev, void *data,
         if (cp->val || test_bit(HCI_FAST_CONNECTABLE, &hdev->dev_flags))
                 write_fast_connectable(&req, false);
 
+        if (test_bit(HCI_ADVERTISING, &hdev->dev_flags) &&
+            hci_conn_num(hdev, LE_LINK) == 0) {
+                disable_advertising(&req);
+                enable_advertising(&req);
+        }
+
         err = hci_req_run(&req, set_connectable_complete);
-        if (err < 0)
+        if (err < 0) {
                 mgmt_pending_remove(cmd);
+                if (err == -ENODATA)
+                        err = send_settings_rsp(sk, MGMT_OP_SET_CONNECTABLE,
+                                                hdev);
+                goto failed;
+        }
 
 failed:
         hci_dev_unlock(hdev);
@@ -1196,6 +1311,7 @@ static int set_pairable(struct sock *sk, struct hci_dev *hdev, void *data,
                         u16 len)
 {
         struct mgmt_mode *cp = data;
+        bool changed;
         int err;
 
         BT_DBG("request for %s", hdev->name);
@@ -1207,17 +1323,18 @@ static int set_pairable(struct sock *sk, struct hci_dev *hdev, void *data,
         hci_dev_lock(hdev);
 
         if (cp->val)
-                set_bit(HCI_PAIRABLE, &hdev->dev_flags);
+                changed = !test_and_set_bit(HCI_PAIRABLE, &hdev->dev_flags);
         else
-                clear_bit(HCI_PAIRABLE, &hdev->dev_flags);
+                changed = test_and_clear_bit(HCI_PAIRABLE, &hdev->dev_flags);
 
         err = send_settings_rsp(sk, MGMT_OP_SET_PAIRABLE, hdev);
         if (err < 0)
-                goto failed;
+                goto unlock;
 
-        err = new_settings(hdev, sk);
+        if (changed)
+                err = new_settings(hdev, sk);
 
-failed:
+unlock:
         hci_dev_unlock(hdev);
         return err;
 }
@@ -1296,7 +1413,7 @@ static int set_ssp(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
 {
         struct mgmt_mode *cp = data;
         struct pending_cmd *cmd;
-        u8 val, status;
+        u8 status;
         int err;
 
         BT_DBG("request for %s", hdev->name);
@@ -1315,14 +1432,20 @@ static int set_ssp(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
 
         hci_dev_lock(hdev);
 
-        val = !!cp->val;
-
         if (!hdev_is_powered(hdev)) {
-                bool changed = false;
+                bool changed;
 
-                if (val != test_bit(HCI_SSP_ENABLED, &hdev->dev_flags)) {
-                        change_bit(HCI_SSP_ENABLED, &hdev->dev_flags);
-                        changed = true;
+                if (cp->val) {
+                        changed = !test_and_set_bit(HCI_SSP_ENABLED,
+                                                    &hdev->dev_flags);
+                } else {
+                        changed = test_and_clear_bit(HCI_SSP_ENABLED,
+                                                     &hdev->dev_flags);
+                        if (!changed)
+                                changed = test_and_clear_bit(HCI_HS_ENABLED,
+                                                             &hdev->dev_flags);
+                        else
+                                clear_bit(HCI_HS_ENABLED, &hdev->dev_flags);
                 }
 
                 err = send_settings_rsp(sk, MGMT_OP_SET_SSP, hdev);
@@ -1335,13 +1458,14 @@ static int set_ssp(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
                 goto failed;
         }
 
-        if (mgmt_pending_find(MGMT_OP_SET_SSP, hdev)) {
+        if (mgmt_pending_find(MGMT_OP_SET_SSP, hdev) ||
+            mgmt_pending_find(MGMT_OP_SET_HS, hdev)) {
                 err = cmd_status(sk, hdev->id, MGMT_OP_SET_SSP,
                                  MGMT_STATUS_BUSY);
                 goto failed;
         }
 
-        if (test_bit(HCI_SSP_ENABLED, &hdev->dev_flags) == val) {
+        if (!!cp->val == test_bit(HCI_SSP_ENABLED, &hdev->dev_flags)) {
                 err = send_settings_rsp(sk, MGMT_OP_SET_SSP, hdev);
                 goto failed;
         }
@@ -1352,7 +1476,7 @@ static int set_ssp(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
                 goto failed;
         }
 
-        err = hci_send_cmd(hdev, HCI_OP_WRITE_SSP_MODE, sizeof(val), &val);
+        err = hci_send_cmd(hdev, HCI_OP_WRITE_SSP_MODE, 1, &cp->val);
         if (err < 0) {
                 mgmt_pending_remove(cmd);
                 goto failed;
@@ -1376,6 +1500,14 @@ static int set_hs(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
         if (status)
                 return cmd_status(sk, hdev->id, MGMT_OP_SET_HS, status);
 
+        if (!lmp_ssp_capable(hdev))
+                return cmd_status(sk, hdev->id, MGMT_OP_SET_HS,
+                                  MGMT_STATUS_NOT_SUPPORTED);
+
+        if (!test_bit(HCI_SSP_ENABLED, &hdev->dev_flags))
+                return cmd_status(sk, hdev->id, MGMT_OP_SET_HS,
+                                  MGMT_STATUS_REJECTED);
+
         if (cp->val != 0x00 && cp->val != 0x01)
                 return cmd_status(sk, hdev->id, MGMT_OP_SET_HS,
                                   MGMT_STATUS_INVALID_PARAMS);
@@ -1463,8 +1595,8 @@ static int set_le(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
                         changed = true;
                 }
 
-                if (!val && test_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags)) {
-                        clear_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags);
+                if (!val && test_bit(HCI_ADVERTISING, &hdev->dev_flags)) {
+                        clear_bit(HCI_ADVERTISING, &hdev->dev_flags);
                         changed = true;
                 }
 
@@ -1500,8 +1632,8 @@ static int set_le(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
 
         hci_req_init(&req, hdev);
 
-        if (test_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags) && !val)
-                hci_req_add(&req, HCI_OP_LE_SET_ADV_ENABLE, sizeof(val), &val);
+        if (test_bit(HCI_ADVERTISING, &hdev->dev_flags) && !val)
+                disable_advertising(&req);
 
         hci_req_add(&req, HCI_OP_WRITE_LE_HOST_SUPPORTED, sizeof(hci_cp),
                     &hci_cp);
@@ -2888,7 +3020,7 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
                         goto failed;
                 }
 
-                if (test_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags)) {
+                if (test_bit(HCI_ADVERTISING, &hdev->dev_flags)) {
                         err = cmd_status(sk, hdev->id, MGMT_OP_START_DISCOVERY,
                                          MGMT_STATUS_REJECTED);
                         mgmt_pending_remove(cmd);
@@ -2906,6 +3038,10 @@ static int start_discovery(struct sock *sk, struct hci_dev *hdev,
                 param_cp.type = LE_SCAN_ACTIVE;
                 param_cp.interval = cpu_to_le16(DISCOV_LE_SCAN_INT);
                 param_cp.window = cpu_to_le16(DISCOV_LE_SCAN_WIN);
+                if (bacmp(&hdev->bdaddr, BDADDR_ANY))
+                        param_cp.own_address_type = ADDR_LE_DEV_PUBLIC;
+                else
+                        param_cp.own_address_type = ADDR_LE_DEV_RANDOM;
                 hci_req_add(&req, HCI_OP_LE_SET_SCAN_PARAM, sizeof(param_cp),
                             &param_cp);
 
@@ -3214,7 +3350,8 @@ static void set_advertising_complete(struct hci_dev *hdev, u8 status)
                 sock_put(match.sk);
 }
 
-static int set_advertising(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
+static int set_advertising(struct sock *sk, struct hci_dev *hdev, void *data,
+                           u16 len)
 {
         struct mgmt_mode *cp = data;
         struct pending_cmd *cmd;
@@ -3236,13 +3373,19 @@ static int set_advertising(struct sock *sk, struct hci_dev *hdev, void *data, u1
         hci_dev_lock(hdev);
 
         val = !!cp->val;
-        enabled = test_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags);
+        enabled = test_bit(HCI_ADVERTISING, &hdev->dev_flags);
 
-        if (!hdev_is_powered(hdev) || val == enabled) {
+        /* The following conditions are ones which mean that we should
+         * not do any HCI communication but directly send a mgmt
+         * response to user space (after toggling the flag if
+         * necessary).
+         */
+        if (!hdev_is_powered(hdev) || val == enabled ||
+            hci_conn_num(hdev, LE_LINK) > 0) {
                 bool changed = false;
 
-                if (val != test_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags)) {
-                        change_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags);
+                if (val != test_bit(HCI_ADVERTISING, &hdev->dev_flags)) {
+                        change_bit(HCI_ADVERTISING, &hdev->dev_flags);
                         changed = true;
                 }
 
@@ -3271,7 +3414,10 @@ static int set_advertising(struct sock *sk, struct hci_dev *hdev, void *data, u1
 
         hci_req_init(&req, hdev);
 
-        hci_req_add(&req, HCI_OP_LE_SET_ADV_ENABLE, sizeof(val), &val);
+        if (val)
+                enable_advertising(&req);
+        else
+                disable_advertising(&req);
 
         err = hci_req_run(&req, set_advertising_complete);
         if (err < 0)
@@ -3322,6 +3468,47 @@ static int set_static_address(struct sock *sk, struct hci_dev *hdev,
         return err;
 }
 
+static int set_scan_params(struct sock *sk, struct hci_dev *hdev,
+                           void *data, u16 len)
+{
+        struct mgmt_cp_set_scan_params *cp = data;
+        __u16 interval, window;
+        int err;
+
+        BT_DBG("%s", hdev->name);
+
+        if (!lmp_le_capable(hdev))
+                return cmd_status(sk, hdev->id, MGMT_OP_SET_SCAN_PARAMS,
+                                  MGMT_STATUS_NOT_SUPPORTED);
+
+        interval = __le16_to_cpu(cp->interval);
+
+        if (interval < 0x0004 || interval > 0x4000)
+                return cmd_status(sk, hdev->id, MGMT_OP_SET_SCAN_PARAMS,
+                                  MGMT_STATUS_INVALID_PARAMS);
+
+        window = __le16_to_cpu(cp->window);
+
+        if (window < 0x0004 || window > 0x4000)
+                return cmd_status(sk, hdev->id, MGMT_OP_SET_SCAN_PARAMS,
+                                  MGMT_STATUS_INVALID_PARAMS);
+
+        if (window > interval)
+                return cmd_status(sk, hdev->id, MGMT_OP_SET_SCAN_PARAMS,
+                                  MGMT_STATUS_INVALID_PARAMS);
+
+        hci_dev_lock(hdev);
+
+        hdev->le_scan_interval = interval;
+        hdev->le_scan_window = window;
+
+        err = cmd_complete(sk, hdev->id, MGMT_OP_SET_SCAN_PARAMS, 0, NULL, 0);
+
+        hci_dev_unlock(hdev);
+
+        return err;
+}
+
 static void fast_connectable_complete(struct hci_dev *hdev, u8 status)
 {
         struct pending_cmd *cmd;
@@ -3420,6 +3607,26 @@ unlock:
         return err;
 }
 
+static void set_bredr_scan(struct hci_request *req)
+{
+        struct hci_dev *hdev = req->hdev;
+        u8 scan = 0;
+
+        /* Ensure that fast connectable is disabled. This function will
+         * not do anything if the page scan parameters are already what
+         * they should be.
+         */
+        write_fast_connectable(req, false);
+
+        if (test_bit(HCI_CONNECTABLE, &hdev->dev_flags))
+                scan |= SCAN_PAGE;
+        if (test_bit(HCI_DISCOVERABLE, &hdev->dev_flags))
+                scan |= SCAN_INQUIRY;
+
+        if (scan)
+                hci_req_add(req, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
+}
+
 static void set_bredr_complete(struct hci_dev *hdev, u8 status)
 {
         struct pending_cmd *cmd;
@@ -3482,7 +3689,6 @@ static int set_bredr(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
 
         if (!hdev_is_powered(hdev)) {
                 if (!cp->val) {
-                        clear_bit(HCI_CONNECTABLE, &hdev->dev_flags);
                         clear_bit(HCI_DISCOVERABLE, &hdev->dev_flags);
                         clear_bit(HCI_SSP_ENABLED, &hdev->dev_flags);
                         clear_bit(HCI_LINK_SECURITY, &hdev->dev_flags);
@@ -3525,7 +3731,12 @@ static int set_bredr(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
         set_bit(HCI_BREDR_ENABLED, &hdev->dev_flags);
 
         hci_req_init(&req, hdev);
+
+        if (test_bit(HCI_CONNECTABLE, &hdev->dev_flags))
+                set_bredr_scan(&req);
+
         hci_update_ad(&req);
+
         err = hci_req_run(&req, set_bredr_complete);
         if (err < 0)
                 mgmt_pending_remove(cmd);
@@ -3587,15 +3798,19 @@ static int load_long_term_keys(struct sock *sk, struct hci_dev *hdev,
 
         for (i = 0; i < key_count; i++) {
                 struct mgmt_ltk_info *key = &cp->keys[i];
-                u8 type;
+                u8 type, addr_type;
+
+                if (key->addr.type == BDADDR_LE_PUBLIC)
+                        addr_type = ADDR_LE_DEV_PUBLIC;
+                else
+                        addr_type = ADDR_LE_DEV_RANDOM;
 
                 if (key->master)
                         type = HCI_SMP_LTK;
                 else
                         type = HCI_SMP_LTK_SLAVE;
 
-                hci_add_ltk(hdev, &key->addr.bdaddr,
-                            bdaddr_to_le(key->addr.type),
+                hci_add_ltk(hdev, &key->addr.bdaddr, addr_type,
                             type, 0, key->authenticated, key->val,
                             key->enc_size, key->ediv, key->rand);
         }
@@ -3658,6 +3873,7 @@ static const struct mgmt_handler {
         { set_advertising,        false, MGMT_SETTING_SIZE },
         { set_bredr,              false, MGMT_SETTING_SIZE },
         { set_static_address,     false, MGMT_SET_STATIC_ADDRESS_SIZE },
+        { set_scan_params,        false, MGMT_SET_SCAN_PARAMS_SIZE },
 };
 
 
@@ -3703,7 +3919,8 @@ int mgmt_control(struct sock *sk, struct msghdr *msg, size_t msglen)
                         goto done;
                 }
 
-                if (test_bit(HCI_USER_CHANNEL, &hdev->dev_flags)) {
+                if (test_bit(HCI_SETUP, &hdev->dev_flags) ||
+                    test_bit(HCI_USER_CHANNEL, &hdev->dev_flags)) {
                         err = cmd_status(sk, index, opcode,
                                          MGMT_STATUS_INVALID_INDEX);
                         goto done;
@@ -3753,44 +3970,24 @@ done:
         return err;
 }
 
-int mgmt_index_added(struct hci_dev *hdev)
+void mgmt_index_added(struct hci_dev *hdev)
 {
-        if (!mgmt_valid_hdev(hdev))
-                return -ENOTSUPP;
+        if (hdev->dev_type != HCI_BREDR)
+                return;
 
-        return mgmt_event(MGMT_EV_INDEX_ADDED, hdev, NULL, 0, NULL);
+        mgmt_event(MGMT_EV_INDEX_ADDED, hdev, NULL, 0, NULL);
 }
 
-int mgmt_index_removed(struct hci_dev *hdev)
+void mgmt_index_removed(struct hci_dev *hdev)
 {
         u8 status = MGMT_STATUS_INVALID_INDEX;
 
-        if (!mgmt_valid_hdev(hdev))
-                return -ENOTSUPP;
+        if (hdev->dev_type != HCI_BREDR)
+                return;
 
         mgmt_pending_foreach(0, hdev, cmd_status_rsp, &status);
 
-        return mgmt_event(MGMT_EV_INDEX_REMOVED, hdev, NULL, 0, NULL);
-}
-
-static void set_bredr_scan(struct hci_request *req)
-{
-        struct hci_dev *hdev = req->hdev;
-        u8 scan = 0;
-
-        /* Ensure that fast connectable is disabled. This function will
-         * not do anything if the page scan parameters are already what
-         * they should be.
-         */
-        write_fast_connectable(req, false);
-
-        if (test_bit(HCI_CONNECTABLE, &hdev->dev_flags))
-                scan |= SCAN_PAGE;
-        if (test_bit(HCI_DISCOVERABLE, &hdev->dev_flags))
-                scan |= SCAN_INQUIRY;
-
-        if (scan)
-                hci_req_add(req, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan);
+        mgmt_event(MGMT_EV_INDEX_REMOVED, hdev, NULL, 0, NULL);
 }
 
 static void powered_complete(struct hci_dev *hdev, u8 status)
@@ -3849,12 +4046,9 @@ static int powered_update_hci(struct hci_dev *hdev)
                 if (bacmp(&hdev->static_addr, BDADDR_ANY))
                         hci_req_add(&req, HCI_OP_LE_SET_RANDOM_ADDR, 6,
                                     &hdev->static_addr);
-        }
-
-        if (test_bit(HCI_LE_PERIPHERAL, &hdev->dev_flags)) {
-                u8 adv = 0x01;
 
-                hci_req_add(&req, HCI_OP_LE_SET_ADV_ENABLE, sizeof(adv), &adv);
+                if (test_bit(HCI_ADVERTISING, &hdev->dev_flags))
+                        enable_advertising(&req);
         }
 
         link_sec = test_bit(HCI_LINK_SECURITY, &hdev->dev_flags);
@@ -3908,33 +4102,37 @@ new_settings:
         return err;
 }
 
-int mgmt_set_powered_failed(struct hci_dev *hdev, int err)
+void mgmt_set_powered_failed(struct hci_dev *hdev, int err)
 {
         struct pending_cmd *cmd;
         u8 status;
 
         cmd = mgmt_pending_find(MGMT_OP_SET_POWERED, hdev);
         if (!cmd)
-                return -ENOENT;
+                return;
 
         if (err == -ERFKILL)
                 status = MGMT_STATUS_RFKILLED;
         else
                 status = MGMT_STATUS_FAILED;
 
-        err = cmd_status(cmd->sk, hdev->id, MGMT_OP_SET_POWERED, status);
+        cmd_status(cmd->sk, hdev->id, MGMT_OP_SET_POWERED, status);
 
         mgmt_pending_remove(cmd);
-
-        return err;
 }
 
 int mgmt_discoverable(struct hci_dev *hdev, u8 discoverable)
 {
-        struct cmd_lookup match = { NULL, hdev };
         bool changed = false;
         int err = 0;
 
+        /* Nothing needed here if there's a pending command since that
+         * commands request completion callback takes care of everything
+         * necessary.
+         */
+        if (mgmt_pending_find(MGMT_OP_SET_DISCOVERABLE, hdev))
+                return 0;
+
         if (discoverable) {
                 if (!test_and_set_bit(HCI_DISCOVERABLE, &hdev->dev_flags))
                         changed = true;
@@ -3943,24 +4141,24 @@ int mgmt_discoverable(struct hci_dev *hdev, u8 discoverable)
                         changed = true;
         }
 
-        mgmt_pending_foreach(MGMT_OP_SET_DISCOVERABLE, hdev, settings_rsp,
-                             &match);
-
         if (changed)
-                err = new_settings(hdev, match.sk);
-
-        if (match.sk)
-                sock_put(match.sk);
+                err = new_settings(hdev, NULL);
 
         return err;
 }
 
 int mgmt_connectable(struct hci_dev *hdev, u8 connectable)
 {
-        struct pending_cmd *cmd;
         bool changed = false;
         int err = 0;
 
+        /* Nothing needed here if there's a pending command since that
+         * commands request completion callback takes care of everything
+         * necessary.
+         */
+        if (mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, hdev))
+                return 0;
+
         if (connectable) {
                 if (!test_and_set_bit(HCI_CONNECTABLE, &hdev->dev_flags))
                         changed = true;
@@ -3969,10 +4167,8 @@ int mgmt_connectable(struct hci_dev *hdev, u8 connectable)
                         changed = true;
         }
 
-        cmd = mgmt_pending_find(MGMT_OP_SET_CONNECTABLE, hdev);
-
         if (changed)
-                err = new_settings(hdev, cmd ? cmd->sk : NULL);
+                err = new_settings(hdev, NULL);
 
         return err;
 }
@@ -4032,9 +4228,9 @@ int mgmt_new_ltk(struct hci_dev *hdev, struct smp_ltk *key, u8 persistent)
                           NULL);
 }
 
-int mgmt_device_connected(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
-                          u8 addr_type, u32 flags, u8 *name, u8 name_len,
-                          u8 *dev_class)
+void mgmt_device_connected(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
+                           u8 addr_type, u32 flags, u8 *name, u8 name_len,
+                           u8 *dev_class)
 {
         char buf[512];
         struct mgmt_ev_device_connected *ev = (void *) buf;
@@ -4055,8 +4251,8 @@ int mgmt_device_connected(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
 
         ev->eir_len = cpu_to_le16(eir_len);
 
-        return mgmt_event(MGMT_EV_DEVICE_CONNECTED, hdev, buf,
-                          sizeof(*ev) + eir_len, NULL);
+        mgmt_event(MGMT_EV_DEVICE_CONNECTED, hdev, buf,
+                    sizeof(*ev) + eir_len, NULL);
 }
 
 static void disconnect_rsp(struct pending_cmd *cmd, void *data)
@@ -4094,12 +4290,11 @@ static void unpair_device_rsp(struct pending_cmd *cmd, void *data)
         mgmt_pending_remove(cmd);
 }
 
-int mgmt_device_disconnected(struct hci_dev *hdev, bdaddr_t *bdaddr,
-                             u8 link_type, u8 addr_type, u8 reason)
+void mgmt_device_disconnected(struct hci_dev *hdev, bdaddr_t *bdaddr,
+                              u8 link_type, u8 addr_type, u8 reason)
 {
         struct mgmt_ev_device_disconnected ev;
         struct sock *sk = NULL;
-        int err;
 
         mgmt_pending_foreach(MGMT_OP_DISCONNECT, hdev, disconnect_rsp, &sk);
 
@@ -4107,45 +4302,39 @@ int mgmt_device_disconnected(struct hci_dev *hdev, bdaddr_t *bdaddr,
         ev.addr.type = link_to_bdaddr(link_type, addr_type);
         ev.reason = reason;
 
-        err = mgmt_event(MGMT_EV_DEVICE_DISCONNECTED, hdev, &ev, sizeof(ev),
-                         sk);
+        mgmt_event(MGMT_EV_DEVICE_DISCONNECTED, hdev, &ev, sizeof(ev), sk);
 
         if (sk)
                 sock_put(sk);
 
         mgmt_pending_foreach(MGMT_OP_UNPAIR_DEVICE, hdev, unpair_device_rsp,
                              hdev);
-
-        return err;
 }
 
-int mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr,
-                           u8 link_type, u8 addr_type, u8 status)
+void mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr,
+                            u8 link_type, u8 addr_type, u8 status)
 {
         struct mgmt_rp_disconnect rp;
         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;
+                return;
 
         bacpy(&rp.addr.bdaddr, bdaddr);
         rp.addr.type = link_to_bdaddr(link_type, addr_type);
 
-        err = cmd_complete(cmd->sk, cmd->index, MGMT_OP_DISCONNECT,
-                           mgmt_status(status), &rp, sizeof(rp));
+        cmd_complete(cmd->sk, cmd->index, MGMT_OP_DISCONNECT,
+                     mgmt_status(status), &rp, sizeof(rp));
 
         mgmt_pending_remove(cmd);
-
-        return err;
 }
 
-int mgmt_connect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
-                        u8 addr_type, u8 status)
+void mgmt_connect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
+                         u8 addr_type, u8 status)
 {
         struct mgmt_ev_connect_failed ev;
 
@@ -4153,7 +4342,7 @@ int mgmt_connect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
         ev.addr.type = link_to_bdaddr(link_type, addr_type);
         ev.status = mgmt_status(status);
 
-        return mgmt_event(MGMT_EV_CONNECT_FAILED, hdev, &ev, sizeof(ev), NULL);
+        mgmt_event(MGMT_EV_CONNECT_FAILED, hdev, &ev, sizeof(ev), NULL);
 }
 
 int mgmt_pin_code_request(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 secure)
@@ -4382,8 +4571,10 @@ int mgmt_ssp_enable_complete(struct hci_dev *hdev, u8 enable, u8 status)
                 u8 mgmt_err = mgmt_status(status);
 
                 if (enable && test_and_clear_bit(HCI_SSP_ENABLED,
-                                                 &hdev->dev_flags))
+                                                 &hdev->dev_flags)) {
+                        clear_bit(HCI_HS_ENABLED, &hdev->dev_flags);
                         err = new_settings(hdev, NULL);
+                }
 
                 mgmt_pending_foreach(MGMT_OP_SET_SSP, hdev, cmd_status_rsp,
                                      &mgmt_err);
@@ -4392,11 +4583,14 @@ int mgmt_ssp_enable_complete(struct hci_dev *hdev, u8 enable, u8 status)
         }
 
         if (enable) {
-                if (!test_and_set_bit(HCI_SSP_ENABLED, &hdev->dev_flags))
-                        changed = true;
+                changed = !test_and_set_bit(HCI_SSP_ENABLED, &hdev->dev_flags);
         } else {
-                if (test_and_clear_bit(HCI_SSP_ENABLED, &hdev->dev_flags))
-                        changed = true;
+                changed = test_and_clear_bit(HCI_SSP_ENABLED, &hdev->dev_flags);
+                if (!changed)
+                        changed = test_and_clear_bit(HCI_HS_ENABLED,
+                                                     &hdev->dev_flags);
+                else
+                        clear_bit(HCI_HS_ENABLED, &hdev->dev_flags);
         }
 
         mgmt_pending_foreach(MGMT_OP_SET_SSP, hdev, settings_rsp, &match);
@@ -4507,20 +4701,20 @@ int mgmt_read_local_oob_data_reply_complete(struct hci_dev *hdev, u8 *hash,
         return err;
 }
 
-int mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
-                      u8 addr_type, u8 *dev_class, s8 rssi, u8 cfm_name, u8
-                      ssp, u8 *eir, u16 eir_len)
+void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
+                       u8 addr_type, u8 *dev_class, s8 rssi, u8 cfm_name, u8
+                       ssp, u8 *eir, u16 eir_len)
 {
         char buf[512];
         struct mgmt_ev_device_found *ev = (void *) buf;
         size_t ev_size;
 
         if (!hci_discovery_active(hdev))
-                return -EPERM;
+                return;
 
         /* Leave 5 bytes for a potential CoD field */
         if (sizeof(*ev) + eir_len + 5 > sizeof(buf))
-                return -EINVAL;
+                return;
 
         memset(buf, 0, sizeof(buf));
 
@@ -4542,11 +4736,11 @@ int mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
         ev->eir_len = cpu_to_le16(eir_len);
         ev_size = sizeof(*ev) + eir_len;
 
-        return mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, ev_size, NULL);
+        mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, ev_size, NULL);
 }
 
-int mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
-                     u8 addr_type, s8 rssi, u8 *name, u8 name_len)
+void mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
+                      u8 addr_type, s8 rssi, u8 *name, u8 name_len)
 {
         struct mgmt_ev_device_found *ev;
         char buf[sizeof(*ev) + HCI_MAX_NAME_LENGTH + 2];
@@ -4565,11 +4759,10 @@ int mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type,
 
         ev->eir_len = cpu_to_le16(eir_len);
 
-        return mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev,
-                          sizeof(*ev) + eir_len, NULL);
+        mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, sizeof(*ev) + eir_len, NULL);
 }
 
-int mgmt_discovering(struct hci_dev *hdev, u8 discovering)
+void mgmt_discovering(struct hci_dev *hdev, u8 discovering)
 {
         struct mgmt_ev_discovering ev;
         struct pending_cmd *cmd;
@@ -4593,7 +4786,7 @@ int mgmt_discovering(struct hci_dev *hdev, u8 discovering)
         ev.type = hdev->discovery.type;
         ev.discovering = discovering;
 
-        return mgmt_event(MGMT_EV_DISCOVERING, hdev, &ev, sizeof(ev), NULL);
+        mgmt_event(MGMT_EV_DISCOVERING, hdev, &ev, sizeof(ev), NULL);
 }
 
 int mgmt_device_blocked(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
@@ -4623,3 +4816,36 @@ int mgmt_device_unblocked(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type)
         return mgmt_event(MGMT_EV_DEVICE_UNBLOCKED, hdev, &ev, sizeof(ev),
                           cmd ? cmd->sk : NULL);
 }
+
+static void adv_enable_complete(struct hci_dev *hdev, u8 status)
+{
+        BT_DBG("%s status %u", hdev->name, status);
+
+        /* Clear the advertising mgmt setting if we failed to re-enable it */
+        if (status) {
+                clear_bit(HCI_ADVERTISING, &hdev->dev_flags);
+                new_settings(hdev, NULL);
+        }
+}
+
+void mgmt_reenable_advertising(struct hci_dev *hdev)
+{
+        struct hci_request req;
+
+        if (hci_conn_num(hdev, LE_LINK) > 0)
+                return;
+
+        if (!test_bit(HCI_ADVERTISING, &hdev->dev_flags))
+                return;
+
+        hci_req_init(&req, hdev);
+        enable_advertising(&req);
+
+        /* If this fails we have no option but to let user space know
+         * that we've disabled advertising.
+         */
+        if (hci_req_run(&req, adv_enable_complete) < 0) {
+                clear_bit(HCI_ADVERTISING, &hdev->dev_flags);
+                new_settings(hdev, NULL);
+        }
+}
diff --git a/net/bluetooth/rfcomm/core.c b/net/bluetooth/rfcomm/core.c
index ca957d34b0c8..27e936a7ddd9 100644
--- a/net/bluetooth/rfcomm/core.c
+++ b/net/bluetooth/rfcomm/core.c
@@ -641,13 +641,13 @@ static struct rfcomm_session *rfcomm_session_get(bdaddr_t *src, bdaddr_t *dst)
 {
         struct rfcomm_session *s;
         struct list_head *p, *n;
-        struct bt_sock *sk;
+        struct l2cap_chan *chan;
         list_for_each_safe(p, n, &session_list) {
                 s = list_entry(p, struct rfcomm_session, list);
-                sk = bt_sk(s->sock->sk);
+                chan = l2cap_pi(s->sock->sk)->chan;
 
-                if ((!bacmp(src, BDADDR_ANY) || !bacmp(&sk->src, src)) &&
-                                !bacmp(&sk->dst, dst))
+                if ((!bacmp(src, BDADDR_ANY) || !bacmp(&chan->src, src)) &&
+                    !bacmp(&chan->dst, dst))
                         return s;
         }
         return NULL;
@@ -732,11 +732,11 @@ failed:
 
 void rfcomm_session_getaddr(struct rfcomm_session *s, bdaddr_t *src, bdaddr_t *dst)
 {
-        struct sock *sk = s->sock->sk;
+        struct l2cap_chan *chan = l2cap_pi(s->sock->sk)->chan;
         if (src)
-                bacpy(src, &bt_sk(sk)->src);
+                bacpy(src, &chan->src);
         if (dst)
-                bacpy(dst, &bt_sk(sk)->dst);
+                bacpy(dst, &chan->dst);
 }
 
 /* ---- RFCOMM frame sending ---- */
@@ -2112,12 +2112,11 @@ static int rfcomm_dlc_debugfs_show(struct seq_file *f, void *x)
         rfcomm_lock();
 
         list_for_each_entry(s, &session_list, list) {
+                struct l2cap_chan *chan = l2cap_pi(s->sock->sk)->chan;
                 struct rfcomm_dlc *d;
                 list_for_each_entry(d, &s->dlcs, list) {
-                        struct sock *sk = s->sock->sk;
-
                         seq_printf(f, "%pMR %pMR %ld %d %d %d %d\n",
-                                   &bt_sk(sk)->src, &bt_sk(sk)->dst,
+                                   &chan->src, &chan->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 072938dc527d..df17276eb32b 100644
--- a/net/bluetooth/rfcomm/sock.c
+++ b/net/bluetooth/rfcomm/sock.c
@@ -87,7 +87,8 @@ static void rfcomm_sk_state_change(struct rfcomm_dlc *d, int err)
                 parent->sk_data_ready(parent, 0);
         } else {
                 if (d->state == BT_CONNECTED)
-                        rfcomm_session_getaddr(d->session, &bt_sk(sk)->src, NULL);
+                        rfcomm_session_getaddr(d->session,
+                                               &rfcomm_pi(sk)->src, NULL);
                 sk->sk_state_change(sk);
         }
 
@@ -110,7 +111,7 @@ static struct sock *__rfcomm_get_sock_by_addr(u8 channel, bdaddr_t *src)
 
         sk_for_each(sk, &rfcomm_sk_list.head) {
                 if (rfcomm_pi(sk)->channel == channel &&
-                                !bacmp(&bt_sk(sk)->src, src))
+                                !bacmp(&rfcomm_pi(sk)->src, src))
                         break;
         }
 
@@ -132,11 +133,11 @@ static struct sock *rfcomm_get_sock_by_channel(int state, u8 channel, bdaddr_t *
 
                 if (rfcomm_pi(sk)->channel == channel) {
                         /* Exact match. */
-                        if (!bacmp(&bt_sk(sk)->src, src))
+                        if (!bacmp(&rfcomm_pi(sk)->src, src))
                                 break;
 
                         /* Closest match */
-                        if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
+                        if (!bacmp(&rfcomm_pi(sk)->src, BDADDR_ANY))
                                 sk1 = sk;
                 }
         }
@@ -355,7 +356,7 @@ static int rfcomm_sock_bind(struct socket *sock, struct sockaddr *addr, int addr
                 err = -EADDRINUSE;
         } else {
                 /* Save source address */
-                bacpy(&bt_sk(sk)->src, &sa->rc_bdaddr);
+                bacpy(&rfcomm_pi(sk)->src, &sa->rc_bdaddr);
                 rfcomm_pi(sk)->channel = sa->rc_channel;
                 sk->sk_state = BT_BOUND;
         }
@@ -393,13 +394,14 @@ static int rfcomm_sock_connect(struct socket *sock, struct sockaddr *addr, int a
         }
 
         sk->sk_state = BT_CONNECT;
-        bacpy(&bt_sk(sk)->dst, &sa->rc_bdaddr);
+        bacpy(&rfcomm_pi(sk)->dst, &sa->rc_bdaddr);
         rfcomm_pi(sk)->channel = sa->rc_channel;
 
         d->sec_level = rfcomm_pi(sk)->sec_level;
         d->role_switch = rfcomm_pi(sk)->role_switch;
 
-        err = rfcomm_dlc_open(d, &bt_sk(sk)->src, &sa->rc_bdaddr, sa->rc_channel);
+        err = rfcomm_dlc_open(d, &rfcomm_pi(sk)->src, &sa->rc_bdaddr,
+                              sa->rc_channel);
         if (!err)
                 err = bt_sock_wait_state(sk, BT_CONNECTED,
                                 sock_sndtimeo(sk, flags & O_NONBLOCK));
@@ -429,7 +431,7 @@ static int rfcomm_sock_listen(struct socket *sock, int backlog)
         }
 
         if (!rfcomm_pi(sk)->channel) {
-                bdaddr_t *src = &bt_sk(sk)->src;
+                bdaddr_t *src = &rfcomm_pi(sk)->src;
                 u8 channel;
 
                 err = -EINVAL;
@@ -530,9 +532,9 @@ static int rfcomm_sock_getname(struct socket *sock, struct sockaddr *addr, int *
         sa->rc_family  = AF_BLUETOOTH;
         sa->rc_channel = rfcomm_pi(sk)->channel;
         if (peer)
-                bacpy(&sa->rc_bdaddr, &bt_sk(sk)->dst);
+                bacpy(&sa->rc_bdaddr, &rfcomm_pi(sk)->dst);
         else
-                bacpy(&sa->rc_bdaddr, &bt_sk(sk)->src);
+                bacpy(&sa->rc_bdaddr, &rfcomm_pi(sk)->src);
 
         *len = sizeof(struct sockaddr_rc);
         return 0;
@@ -951,8 +953,8 @@ int rfcomm_connect_ind(struct rfcomm_session *s, u8 channel, struct rfcomm_dlc *
         bt_sock_reclassify_lock(sk, BTPROTO_RFCOMM);
 
         rfcomm_sock_init(sk, parent);
-        bacpy(&bt_sk(sk)->src, &src);
-        bacpy(&bt_sk(sk)->dst, &dst);
+        bacpy(&rfcomm_pi(sk)->src, &src);
+        bacpy(&rfcomm_pi(sk)->dst, &dst);
         rfcomm_pi(sk)->channel = channel;
 
         sk->sk_state = BT_CONFIG;
@@ -979,7 +981,7 @@ static int rfcomm_sock_debugfs_show(struct seq_file *f, void *p)
 
         sk_for_each(sk, &rfcomm_sk_list.head) {
                 seq_printf(f, "%pMR %pMR %d %d\n",
-                           &bt_sk(sk)->src, &bt_sk(sk)->dst,
+                           &rfcomm_pi(sk)->src, &rfcomm_pi(sk)->dst,
                            sk->sk_state, rfcomm_pi(sk)->channel);
         }
 
diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c
index 96bd388d93a4..a92aebac56ca 100644
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -92,9 +92,6 @@ static struct sco_conn *sco_conn_add(struct hci_conn *hcon)
         hcon->sco_data = conn;
         conn->hcon = hcon;
 
-        conn->src = &hdev->bdaddr;
-        conn->dst = &hcon->dst;
-
         if (hdev->sco_mtu > 0)
                 conn->mtu = hdev->sco_mtu;
         else
@@ -156,16 +153,14 @@ static int sco_chan_add(struct sco_conn *conn, struct sock *sk,
 
 static int sco_connect(struct sock *sk)
 {
-        bdaddr_t *src = &bt_sk(sk)->src;
-        bdaddr_t *dst = &bt_sk(sk)->dst;
         struct sco_conn *conn;
         struct hci_conn *hcon;
         struct hci_dev  *hdev;
         int err, type;
 
-        BT_DBG("%pMR -> %pMR", src, dst);
+        BT_DBG("%pMR -> %pMR", &sco_pi(sk)->src, &sco_pi(sk)->dst);
 
-        hdev = hci_get_route(dst, src);
+        hdev = hci_get_route(&sco_pi(sk)->dst, &sco_pi(sk)->src);
         if (!hdev)
                 return -EHOSTUNREACH;
 
@@ -182,7 +177,8 @@ static int sco_connect(struct sock *sk)
                 goto done;
         }
 
-        hcon = hci_connect_sco(hdev, type, dst, sco_pi(sk)->setting);
+        hcon = hci_connect_sco(hdev, type, &sco_pi(sk)->dst,
+                               sco_pi(sk)->setting);
         if (IS_ERR(hcon)) {
                 err = PTR_ERR(hcon);
                 goto done;
@@ -196,7 +192,7 @@ static int sco_connect(struct sock *sk)
         }
 
         /* Update source addr of the socket */
-        bacpy(src, conn->src);
+        bacpy(&sco_pi(sk)->src, &hcon->src);
 
         err = sco_chan_add(conn, sk, NULL);
         if (err)
@@ -270,7 +266,7 @@ static struct sock *__sco_get_sock_listen_by_addr(bdaddr_t *ba)
                 if (sk->sk_state != BT_LISTEN)
                         continue;
 
-                if (!bacmp(&bt_sk(sk)->src, ba))
+                if (!bacmp(&sco_pi(sk)->src, ba))
                         return sk;
         }
 
@@ -291,11 +287,11 @@ static struct sock *sco_get_sock_listen(bdaddr_t *src)
                         continue;
 
                 /* Exact match. */
-                if (!bacmp(&bt_sk(sk)->src, src))
+                if (!bacmp(&sco_pi(sk)->src, src))
                         break;
 
                 /* Closest match */
-                if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY))
+                if (!bacmp(&sco_pi(sk)->src, BDADDR_ANY))
                         sk1 = sk;
         }
 
@@ -475,7 +471,7 @@ static int sco_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_le
                 goto done;
         }
 
-        bacpy(&bt_sk(sk)->src, &sa->sco_bdaddr);
+        bacpy(&sco_pi(sk)->src, &sa->sco_bdaddr);
 
         sk->sk_state = BT_BOUND;
 
@@ -505,7 +501,7 @@ static int sco_sock_connect(struct socket *sock, struct sockaddr *addr, int alen
         lock_sock(sk);
 
         /* Set destination address and psm */
-        bacpy(&bt_sk(sk)->dst, &sa->sco_bdaddr);
+        bacpy(&sco_pi(sk)->dst, &sa->sco_bdaddr);
 
         err = sco_connect(sk);
         if (err)
@@ -522,7 +518,7 @@ done:
 static int sco_sock_listen(struct socket *sock, int backlog)
 {
         struct sock *sk = sock->sk;
-        bdaddr_t *src = &bt_sk(sk)->src;
+        bdaddr_t *src = &sco_pi(sk)->src;
         int err = 0;
 
         BT_DBG("sk %p backlog %d", sk, backlog);
@@ -626,9 +622,9 @@ static int sco_sock_getname(struct socket *sock, struct sockaddr *addr, int *len
         *len = sizeof(struct sockaddr_sco);
 
         if (peer)
-                bacpy(&sa->sco_bdaddr, &bt_sk(sk)->dst);
+                bacpy(&sa->sco_bdaddr, &sco_pi(sk)->dst);
         else
-                bacpy(&sa->sco_bdaddr, &bt_sk(sk)->src);
+                bacpy(&sa->sco_bdaddr, &sco_pi(sk)->src);
 
         return 0;
 }
@@ -999,7 +995,7 @@ static void sco_conn_ready(struct sco_conn *conn)
         } else {
                 sco_conn_lock(conn);
 
-                parent = sco_get_sock_listen(conn->src);
+                parent = sco_get_sock_listen(&conn->hcon->src);
                 if (!parent) {
                         sco_conn_unlock(conn);
                         return;
@@ -1017,8 +1013,8 @@ static void sco_conn_ready(struct sco_conn *conn)
 
                 sco_sock_init(sk, parent);
 
-                bacpy(&bt_sk(sk)->src, conn->src);
-                bacpy(&bt_sk(sk)->dst, conn->dst);
+                bacpy(&sco_pi(sk)->src, &conn->hcon->src);
+                bacpy(&sco_pi(sk)->dst, &conn->hcon->dst);
 
                 hci_conn_hold(conn->hcon);
                 __sco_chan_add(conn, sk, parent);
@@ -1051,8 +1047,8 @@ int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, __u8 *flags)
                 if (sk->sk_state != BT_LISTEN)
                         continue;
 
-                if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr) ||
-                    !bacmp(&bt_sk(sk)->src, BDADDR_ANY)) {
+                if (!bacmp(&sco_pi(sk)->src, &hdev->bdaddr) ||
+                    !bacmp(&sco_pi(sk)->src, BDADDR_ANY)) {
                         lm |= HCI_LM_ACCEPT;
 
                         if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags))
@@ -1111,8 +1107,8 @@ static int sco_debugfs_show(struct seq_file *f, void *p)
         read_lock(&sco_sk_list.lock);
 
         sk_for_each(sk, &sco_sk_list.head) {
-                seq_printf(f, "%pMR %pMR %d\n", &bt_sk(sk)->src,
-                           &bt_sk(sk)->dst, sk->sk_state);
+                seq_printf(f, "%pMR %pMR %d\n", &sco_pi(sk)->src,
+                           &sco_pi(sk)->dst, sk->sk_state);
         }
 
         read_unlock(&sco_sk_list.lock);
diff --git a/net/bluetooth/smp.c b/net/bluetooth/smp.c
index 884b2081a262..463e50c58716 100644
--- a/net/bluetooth/smp.c
+++ b/net/bluetooth/smp.c
@@ -28,7 +28,8 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/mgmt.h>
-#include <net/bluetooth/smp.h>
+
+#include "smp.h"
 
 #define SMP_TIMEOUT     msecs_to_jiffies(30000)
 
@@ -85,8 +86,8 @@ static int smp_e(struct crypto_blkcipher *tfm, const u8 *k, u8 *r)
 }
 
 static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16],
-                u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia,
-                u8 _rat, bdaddr_t *ra, u8 res[16])
+                  u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia,
+                  u8 _rat, bdaddr_t *ra, u8 res[16])
 {
         u8 p1[16], p2[16];
         int err;
@@ -126,8 +127,8 @@ static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16],
         return err;
 }
 
-static int smp_s1(struct crypto_blkcipher *tfm, u8 k[16],
-                        u8 r1[16], u8 r2[16], u8 _r[16])
+static int smp_s1(struct crypto_blkcipher *tfm, u8 k[16], u8 r1[16],
+                  u8 r2[16], u8 _r[16])
 {
         int err;
 
@@ -150,7 +151,7 @@ static int smp_rand(u8 *buf)
 }
 
 static struct sk_buff *smp_build_cmd(struct l2cap_conn *conn, u8 code,
-                                                u16 dlen, void *data)
+                                     u16 dlen, void *data)
 {
         struct sk_buff *skb;
         struct l2cap_hdr *lh;
@@ -213,9 +214,8 @@ static __u8 seclevel_to_authreq(__u8 sec_level)
 }
 
 static void build_pairing_cmd(struct l2cap_conn *conn,
-                                struct smp_cmd_pairing *req,
-                                struct smp_cmd_pairing *rsp,
-                                __u8 authreq)
+                              struct smp_cmd_pairing *req,
+                              struct smp_cmd_pairing *rsp, __u8 authreq)
 {
         u8 dist_keys = 0;
 
@@ -249,7 +249,7 @@ static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size)
         struct smp_chan *smp = conn->smp_chan;
 
         if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) ||
-                        (max_key_size < SMP_MIN_ENC_KEY_SIZE))
+            (max_key_size < SMP_MIN_ENC_KEY_SIZE))
                 return SMP_ENC_KEY_SIZE;
 
         smp->enc_key_size = max_key_size;
@@ -263,15 +263,15 @@ static void smp_failure(struct l2cap_conn *conn, u8 reason, u8 send)
 
         if (send)
                 smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason),
-                                                                &reason);
+                             &reason);
 
-        clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->hcon->flags);
-        mgmt_auth_failed(conn->hcon->hdev, conn->dst, hcon->type,
-                         hcon->dst_type, HCI_ERROR_AUTH_FAILURE);
+        clear_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags);
+        mgmt_auth_failed(hcon->hdev, &hcon->dst, hcon->type, hcon->dst_type,
+                         HCI_ERROR_AUTH_FAILURE);
 
         cancel_delayed_work_sync(&conn->security_timer);
 
-        if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags))
+        if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags))
                 smp_chan_destroy(conn);
 }
 
@@ -309,8 +309,8 @@ static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth,
         /* If either side has unknown io_caps, use JUST WORKS */
         /* Otherwise, look up method from the table */
         if (!(auth & SMP_AUTH_MITM) ||
-                        local_io > SMP_IO_KEYBOARD_DISPLAY ||
-                        remote_io > SMP_IO_KEYBOARD_DISPLAY)
+            local_io > SMP_IO_KEYBOARD_DISPLAY ||
+            remote_io > SMP_IO_KEYBOARD_DISPLAY)
                 method = JUST_WORKS;
         else
                 method = gen_method[remote_io][local_io];
@@ -354,10 +354,10 @@ static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth,
         hci_dev_lock(hcon->hdev);
 
         if (method == REQ_PASSKEY)
-                ret = mgmt_user_passkey_request(hcon->hdev, conn->dst,
+                ret = mgmt_user_passkey_request(hcon->hdev, &hcon->dst,
                                                 hcon->type, hcon->dst_type);
         else
-                ret = mgmt_user_confirm_request(hcon->hdev, conn->dst,
+                ret = mgmt_user_confirm_request(hcon->hdev, &hcon->dst,
                                                 hcon->type, hcon->dst_type,
                                                 cpu_to_le32(passkey), 0);
 
@@ -386,12 +386,13 @@ static void confirm_work(struct work_struct *work)
         smp->tfm = tfm;
 
         if (conn->hcon->out)
-                ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp, 0,
-                             conn->src, conn->hcon->dst_type, conn->dst, res);
+                ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp,
+                             conn->hcon->src_type, &conn->hcon->src,
+                             conn->hcon->dst_type, &conn->hcon->dst, res);
         else
                 ret = smp_c1(tfm, smp->tk, smp->prnd, smp->preq, smp->prsp,
-                             conn->hcon->dst_type, conn->dst, 0, conn->src,
-                             res);
+                             conn->hcon->dst_type, &conn->hcon->dst,
+                             conn->hcon->src_type, &conn->hcon->src, res);
         if (ret) {
                 reason = SMP_UNSPECIFIED;
                 goto error;
@@ -425,11 +426,13 @@ static void random_work(struct work_struct *work)
         BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave");
 
         if (hcon->out)
-                ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp, 0,
-                             conn->src, hcon->dst_type, conn->dst, res);
+                ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp,
+                             hcon->src_type, &hcon->src,
+                             hcon->dst_type, &hcon->dst, res);
         else
                 ret = smp_c1(tfm, smp->tk, smp->rrnd, smp->preq, smp->prsp,
-                             hcon->dst_type, conn->dst, 0, conn->src, res);
+                             hcon->dst_type, &hcon->dst,
+                             hcon->src_type, &hcon->src, res);
         if (ret) {
                 reason = SMP_UNSPECIFIED;
                 goto error;
@@ -477,9 +480,9 @@ static void random_work(struct work_struct *work)
                 swap128(key, stk);
 
                 memset(stk + smp->enc_key_size, 0,
-                                SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size);
+                       SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size);
 
-                hci_add_ltk(hcon->hdev, conn->dst, hcon->dst_type,
+                hci_add_ltk(hcon->hdev, &hcon->dst, hcon->dst_type,
                             HCI_SMP_STK_SLAVE, 0, 0, stk, smp->enc_key_size,
                             ediv, rand);
         }
@@ -494,7 +497,7 @@ static struct smp_chan *smp_chan_create(struct l2cap_conn *conn)
 {
         struct smp_chan *smp;
 
-        smp = kzalloc(sizeof(struct smp_chan), GFP_ATOMIC);
+        smp = kzalloc(sizeof(*smp), GFP_ATOMIC);
         if (!smp)
                 return NULL;
 
@@ -649,7 +652,7 @@ static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb)
         memcpy(&smp->prsp[1], rsp, sizeof(*rsp));
 
         if ((req->auth_req & SMP_AUTH_BONDING) &&
-                        (rsp->auth_req & SMP_AUTH_BONDING))
+            (rsp->auth_req & SMP_AUTH_BONDING))
                 auth = SMP_AUTH_BONDING;
 
         auth |= (req->auth_req | rsp->auth_req) & SMP_AUTH_MITM;
@@ -684,7 +687,7 @@ static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb)
 
                 swap128(smp->prnd, random);
                 smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(random),
-                                                                random);
+                             random);
         } else if (test_bit(SMP_FLAG_TK_VALID, &smp->smp_flags)) {
                 queue_work(hdev->workqueue, &smp->confirm);
         } else {
@@ -714,7 +717,7 @@ static u8 smp_ltk_encrypt(struct l2cap_conn *conn, u8 sec_level)
         struct smp_ltk *key;
         struct hci_conn *hcon = conn->hcon;
 
-        key = hci_find_ltk_by_addr(hcon->hdev, conn->dst, hcon->dst_type);
+        key = hci_find_ltk_by_addr(hcon->hdev, &hcon->dst, hcon->dst_type);
         if (!key)
                 return 0;
 
@@ -728,8 +731,8 @@ static u8 smp_ltk_encrypt(struct l2cap_conn *conn, u8 sec_level)
         hcon->enc_key_size = key->enc_size;
 
         return 1;
-
 }
+
 static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb)
 {
         struct smp_cmd_security_req *rp = (void *) skb->data;
@@ -835,9 +838,9 @@ static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb)
         skb_pull(skb, sizeof(*rp));
 
         hci_dev_lock(hdev);
-        authenticated = (conn->hcon->sec_level == BT_SECURITY_HIGH);
-        hci_add_ltk(conn->hcon->hdev, conn->dst, hcon->dst_type,
-                    HCI_SMP_LTK, 1, authenticated, smp->tk, smp->enc_key_size,
+        authenticated = (hcon->sec_level == BT_SECURITY_HIGH);
+        hci_add_ltk(hdev, &hcon->dst, hcon->dst_type, HCI_SMP_LTK, 1,
+                    authenticated, smp->tk, smp->enc_key_size,
                     rp->ediv, rp->rand);
         smp_distribute_keys(conn, 1);
         hci_dev_unlock(hdev);
@@ -985,7 +988,7 @@ int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
                 smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc);
 
                 authenticated = hcon->sec_level == BT_SECURITY_HIGH;
-                hci_add_ltk(conn->hcon->hdev, conn->dst, hcon->dst_type,
+                hci_add_ltk(hcon->hdev, &hcon->dst, hcon->dst_type,
                             HCI_SMP_LTK_SLAVE, 1, authenticated,
                             enc.ltk, smp->enc_key_size, ediv, ident.rand);
 
@@ -1007,10 +1010,10 @@ int smp_distribute_keys(struct l2cap_conn *conn, __u8 force)
 
                 /* Just public address */
                 memset(&addrinfo, 0, sizeof(addrinfo));
-                bacpy(&addrinfo.bdaddr, conn->src);
+                bacpy(&addrinfo.bdaddr, &conn->hcon->src);
 
                 smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo),
-                                                                &addrinfo);
+                             &addrinfo);
 
                 *keydist &= ~SMP_DIST_ID_KEY;
         }
diff --git a/include/net/bluetooth/smp.h b/net/bluetooth/smp.h
index f8ba07f3e5fa..f8ba07f3e5fa 100644
--- a/include/net/bluetooth/smp.h
+++ b/net/bluetooth/smp.h
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index e73cd0637f3b..fe48b093d4dc 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -334,6 +334,7 @@ enum ieee80211_sta_flags {
         IEEE80211_STA_DISABLE_VHT       = BIT(11),
         IEEE80211_STA_DISABLE_80P80MHZ  = BIT(12),
         IEEE80211_STA_DISABLE_160MHZ    = BIT(13),
+        IEEE80211_STA_DISABLE_WMM       = BIT(14),
 };
 
 struct ieee80211_mgd_auth_data {
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index 91cc8281e266..d7bdc4b97dde 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -2527,7 +2527,7 @@ static bool ieee80211_assoc_success(struct ieee80211_sub_if_data *sdata,
          */
         ifmgd->wmm_last_param_set = -1;
 
-        if (elems.wmm_param)
+        if (!(ifmgd->flags & IEEE80211_STA_DISABLE_WMM) && elems.wmm_param)
                 ieee80211_sta_wmm_params(local, sdata, elems.wmm_param,
                                          elems.wmm_param_len);
         else
@@ -2955,7 +2955,8 @@ static void ieee80211_rx_mgmt_beacon(struct ieee80211_sub_if_data *sdata,
         ieee80211_sta_process_chanswitch(sdata, rx_status->mactime,
                                          &elems, true);
 
-        if (ieee80211_sta_wmm_params(local, sdata, elems.wmm_param,
+        if (!(ifmgd->flags & IEEE80211_STA_DISABLE_WMM) &&
+            ieee80211_sta_wmm_params(local, sdata, elems.wmm_param,
                                      elems.wmm_param_len))
                 changed |= BSS_CHANGED_QOS;
 
@@ -3937,6 +3938,44 @@ int ieee80211_mgd_auth(struct ieee80211_sub_if_data *sdata,
         return err;
 }
 
+static bool ieee80211_usable_wmm_params(struct ieee80211_sub_if_data *sdata,
+                                        const u8 *wmm_param, int len)
+{
+        const u8 *pos;
+        size_t left;
+
+        if (len < 8)
+                return false;
+
+        if (wmm_param[5] != 1 /* version */)
+                return false;
+
+        pos = wmm_param + 8;
+        left = len - 8;
+
+        for (; left >= 4; left -= 4, pos += 4) {
+                u8 aifsn = pos[0] & 0x0f;
+                u8 ecwmin = pos[1] & 0x0f;
+                u8 ecwmax = (pos[1] & 0xf0) >> 4;
+                int aci = (pos[0] >> 5) & 0x03;
+
+                if (aifsn < 2) {
+                        sdata_info(sdata,
+                                   "AP has invalid WMM params (AIFSN=%d for ACI %d), disabling WMM\n",
+                                   aifsn, aci);
+                        return false;
+                }
+                if (ecwmin > ecwmax) {
+                        sdata_info(sdata,
+                                   "AP has invalid WMM params (ECWmin/max=%d/%d for ACI %d), disabling WMM\n",
+                                   ecwmin, ecwmax, aci);
+                        return false;
+                }
+        }
+
+        return true;
+}
+
 int ieee80211_mgd_assoc(struct ieee80211_sub_if_data *sdata,
                         struct cfg80211_assoc_request *req)
 {
@@ -3994,9 +4033,45 @@ int ieee80211_mgd_assoc(struct ieee80211_sub_if_data *sdata,
         }
 
         /* prepare assoc data */
-        
+
         ifmgd->beacon_crc_valid = false;
 
+        assoc_data->wmm = bss->wmm_used &&
+                          (local->hw.queues >= IEEE80211_NUM_ACS);
+        if (assoc_data->wmm) {
+                /* try to check validity of WMM params IE */
+                const struct cfg80211_bss_ies *ies;
+                const u8 *wp, *start, *end;
+
+                rcu_read_lock();
+                ies = rcu_dereference(req->bss->ies);
+                start = ies->data;
+                end = start + ies->len;
+
+                while (true) {
+                        wp = cfg80211_find_vendor_ie(
+                                WLAN_OUI_MICROSOFT,
+                                WLAN_OUI_TYPE_MICROSOFT_WMM,
+                                start, end - start);
+                        if (!wp)
+                                break;
+                        start = wp + wp[1] + 2;
+                        /* if this IE is too short, try the next */
+                        if (wp[1] <= 4)
+                                continue;
+                        /* if this IE is WMM params, we found what we wanted */
+                        if (wp[6] == 1)
+                                break;
+                }
+
+                if (!wp || !ieee80211_usable_wmm_params(sdata, wp + 2,
+                                                        wp[1] - 2)) {
+                        assoc_data->wmm = false;
+                        ifmgd->flags |= IEEE80211_STA_DISABLE_WMM;
+                }
+                rcu_read_unlock();
+        }
+
         /*
          * IEEE802.11n does not allow TKIP/WEP as pairwise ciphers in HT mode.
          * We still associate in non-HT mode (11a/b/g) if any one of these
@@ -4026,18 +4101,22 @@ int ieee80211_mgd_assoc(struct ieee80211_sub_if_data *sdata,
         /* Also disable HT if we don't support it or the AP doesn't use WMM */
         sband = local->hw.wiphy->bands[req->bss->channel->band];
         if (!sband->ht_cap.ht_supported ||
-            local->hw.queues < IEEE80211_NUM_ACS || !bss->wmm_used) {
+            local->hw.queues < IEEE80211_NUM_ACS || !bss->wmm_used ||
+            ifmgd->flags & IEEE80211_STA_DISABLE_WMM) {
                 ifmgd->flags |= IEEE80211_STA_DISABLE_HT;
-                if (!bss->wmm_used)
+                if (!bss->wmm_used &&
+                    !(ifmgd->flags & IEEE80211_STA_DISABLE_WMM))
                         netdev_info(sdata->dev,
                                     "disabling HT as WMM/QoS is not supported by the AP\n");
         }
 
         /* disable VHT if we don't support it or the AP doesn't use WMM */
         if (!sband->vht_cap.vht_supported ||
-            local->hw.queues < IEEE80211_NUM_ACS || !bss->wmm_used) {
+            local->hw.queues < IEEE80211_NUM_ACS || !bss->wmm_used ||
+            ifmgd->flags & IEEE80211_STA_DISABLE_WMM) {
                 ifmgd->flags |= IEEE80211_STA_DISABLE_VHT;
-                if (!bss->wmm_used)
+                if (!bss->wmm_used &&
+                    !(ifmgd->flags & IEEE80211_STA_DISABLE_WMM))
                         netdev_info(sdata->dev,
                                     "disabling VHT as WMM/QoS is not supported by the AP\n");
         }
@@ -4066,8 +4145,6 @@ int ieee80211_mgd_assoc(struct ieee80211_sub_if_data *sdata,
                 sdata->smps_mode = ifmgd->req_smps;
 
         assoc_data->capability = req->bss->capability;
-        assoc_data->wmm = bss->wmm_used &&
-                          (local->hw.queues >= IEEE80211_NUM_ACS);
         assoc_data->supp_rates = bss->supp_rates;
         assoc_data->supp_rates_len = bss->supp_rates_len;
 
diff --git a/net/mac80211/rate.c b/net/mac80211/rate.c
index e126605cec66..22b223f13c9f 100644
--- a/net/mac80211/rate.c
+++ b/net/mac80211/rate.c
@@ -235,7 +235,8 @@ static void rc_send_low_basicrate(s8 *idx, u32 basic_rates,
 static void __rate_control_send_low(struct ieee80211_hw *hw,
                                     struct ieee80211_supported_band *sband,
                                     struct ieee80211_sta *sta,
-                                    struct ieee80211_tx_info *info)
+                                    struct ieee80211_tx_info *info,
+                                    u32 rate_mask)
 {
         int i;
         u32 rate_flags =
@@ -247,6 +248,12 @@ static void __rate_control_send_low(struct ieee80211_hw *hw,
 
         info->control.rates[0].idx = 0;
         for (i = 0; i < sband->n_bitrates; i++) {
+                if (!(rate_mask & BIT(i)))
+                        continue;
+
+                if ((rate_flags & sband->bitrates[i].flags) != rate_flags)
+                        continue;
+
                 if (!rate_supported(sta, sband->band, i))
                         continue;
 
@@ -274,7 +281,8 @@ bool rate_control_send_low(struct ieee80211_sta *pubsta,
         bool use_basicrate = false;
 
         if (!pubsta || !priv_sta || rc_no_data_or_no_ack_use_min(txrc)) {
-                __rate_control_send_low(txrc->hw, sband, pubsta, info);
+                __rate_control_send_low(txrc->hw, sband, pubsta, info,
+                                        txrc->rate_idx_mask);
 
                 if (!pubsta && txrc->bss) {
                         mcast_rate = txrc->bss_conf->mcast_rate[sband->band];
@@ -656,7 +664,8 @@ void ieee80211_get_tx_rates(struct ieee80211_vif *vif,
                 rate_control_apply_mask(sdata, sta, sband, info, dest, max_rates);
 
         if (dest[0].idx < 0)
-                __rate_control_send_low(&sdata->local->hw, sband, sta, info);
+                __rate_control_send_low(&sdata->local->hw, sband, sta, info,
+                                        sdata->rc_rateidx_mask[info->band]);
 
         if (sta)
                 rate_fixup_ratelist(vif, sband, info, dest, max_rates);
diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig
index 5948b2fc72f6..6e0fa0cce198 100644
--- a/net/nfc/Kconfig
+++ b/net/nfc/Kconfig
@@ -14,6 +14,20 @@ menuconfig NFC
           To compile this support as a module, choose M here: the module will
           be called nfc.
 
+config NFC_DIGITAL
+        depends on NFC
+        select CRC_CCITT
+        select CRC_ITU_T
+        tristate "NFC Digital Protocol stack support"
+        default n
+        help
+          Say Y if you want to build NFC digital protocol stack support.
+          This is needed by NFC chipsets whose firmware only implement
+          the NFC analog layer.
+
+          To compile this support as a module, choose M here: the module will
+          be called nfc_digital.
+
 source "net/nfc/nci/Kconfig"
 source "net/nfc/hci/Kconfig"
 
diff --git a/net/nfc/Makefile b/net/nfc/Makefile
index a76f4533cb6c..2555ff8e7219 100644
--- a/net/nfc/Makefile
+++ b/net/nfc/Makefile
@@ -5,7 +5,9 @@
 obj-$(CONFIG_NFC) += nfc.o
 obj-$(CONFIG_NFC_NCI) += nci/
 obj-$(CONFIG_NFC_HCI) += hci/
+obj-$(CONFIG_NFC_DIGITAL) += nfc_digital.o
 
 nfc-objs := core.o netlink.o af_nfc.o rawsock.o llcp_core.o llcp_commands.o \
                 llcp_sock.o
 
+nfc_digital-objs := digital_core.o digital_technology.o digital_dep.o
diff --git a/net/nfc/core.c b/net/nfc/core.c
index e92923cf3e03..872529105abc 100644
--- a/net/nfc/core.c
+++ b/net/nfc/core.c
@@ -384,6 +384,19 @@ int nfc_dep_link_is_up(struct nfc_dev *dev, u32 target_idx,
 {
         dev->dep_link_up = true;
 
+        if (!dev->active_target) {
+                struct nfc_target *target;
+
+                target = nfc_find_target(dev, target_idx);
+                if (target == NULL)
+                        return -ENOTCONN;
+
+                dev->active_target = target;
+        }
+
+        dev->polling = false;
+        dev->rf_mode = rf_mode;
+
         nfc_llcp_mac_is_up(dev, target_idx, comm_mode, rf_mode);
 
         return nfc_genl_dep_link_up_event(dev, target_idx, comm_mode, rf_mode);
@@ -536,7 +549,7 @@ error:
         return rc;
 }
 
-static struct nfc_se *find_se(struct nfc_dev *dev, u32 se_idx)
+struct nfc_se *nfc_find_se(struct nfc_dev *dev, u32 se_idx)
 {
         struct nfc_se *se, *n;
 
@@ -546,6 +559,7 @@ static struct nfc_se *find_se(struct nfc_dev *dev, u32 se_idx)
 
         return NULL;
 }
+EXPORT_SYMBOL(nfc_find_se);
 
 int nfc_enable_se(struct nfc_dev *dev, u32 se_idx)
 {
@@ -577,7 +591,7 @@ int nfc_enable_se(struct nfc_dev *dev, u32 se_idx)
                 goto error;
         }
 
-        se = find_se(dev, se_idx);
+        se = nfc_find_se(dev, se_idx);
         if (!se) {
                 rc = -EINVAL;
                 goto error;
@@ -622,7 +636,7 @@ int nfc_disable_se(struct nfc_dev *dev, u32 se_idx)
                 goto error;
         }
 
-        se = find_se(dev, se_idx);
+        se = nfc_find_se(dev, se_idx);
         if (!se) {
                 rc = -EINVAL;
                 goto error;
@@ -881,7 +895,7 @@ int nfc_add_se(struct nfc_dev *dev, u32 se_idx, u16 type)
 
         pr_debug("%s se index %d\n", dev_name(&dev->dev), se_idx);
 
-        se = find_se(dev, se_idx);
+        se = nfc_find_se(dev, se_idx);
         if (se)
                 return -EALREADY;
 
diff --git a/net/nfc/digital.h b/net/nfc/digital.h
new file mode 100644
index 000000000000..08b29b55ea63
--- /dev/null
+++ b/net/nfc/digital.h
@@ -0,0 +1,170 @@
+/*
+ * NFC Digital Protocol stack
+ * Copyright (c) 2013, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ *
+ */
+
+#ifndef __DIGITAL_H
+#define __DIGITAL_H
+
+#include <net/nfc/nfc.h>
+#include <net/nfc/digital.h>
+
+#include <linux/crc-ccitt.h>
+#include <linux/crc-itu-t.h>
+
+#define PROTOCOL_ERR(req) pr_err("%d: NFC Digital Protocol error: %s\n", \
+                                 __LINE__, req)
+
+#define DIGITAL_CMD_IN_SEND        0
+#define DIGITAL_CMD_TG_SEND        1
+#define DIGITAL_CMD_TG_LISTEN      2
+#define DIGITAL_CMD_TG_LISTEN_MDAA 3
+
+#define DIGITAL_MAX_HEADER_LEN 7
+#define DIGITAL_CRC_LEN        2
+
+#define DIGITAL_SENSF_NFCID2_NFC_DEP_B1 0x01
+#define DIGITAL_SENSF_NFCID2_NFC_DEP_B2 0xFE
+
+#define DIGITAL_SENS_RES_NFC_DEP 0x0100
+#define DIGITAL_SEL_RES_NFC_DEP  0x40
+#define DIGITAL_SENSF_FELICA_SC  0xFFFF
+
+#define DIGITAL_DRV_CAPS_IN_CRC(ddev) \
+        ((ddev)->driver_capabilities & NFC_DIGITAL_DRV_CAPS_IN_CRC)
+#define DIGITAL_DRV_CAPS_TG_CRC(ddev) \
+        ((ddev)->driver_capabilities & NFC_DIGITAL_DRV_CAPS_TG_CRC)
+
+struct digital_data_exch {
+        data_exchange_cb_t cb;
+        void *cb_context;
+};
+
+struct sk_buff *digital_skb_alloc(struct nfc_digital_dev *ddev,
+                                  unsigned int len);
+
+int digital_send_cmd(struct nfc_digital_dev *ddev, u8 cmd_type,
+                     struct sk_buff *skb, struct digital_tg_mdaa_params *params,
+                     u16 timeout, nfc_digital_cmd_complete_t cmd_cb,
+                     void *cb_context);
+
+int digital_in_configure_hw(struct nfc_digital_dev *ddev, int type, int param);
+static inline int digital_in_send_cmd(struct nfc_digital_dev *ddev,
+                                      struct sk_buff *skb, u16 timeout,
+                                      nfc_digital_cmd_complete_t cmd_cb,
+                                      void *cb_context)
+{
+        return digital_send_cmd(ddev, DIGITAL_CMD_IN_SEND, skb, NULL, timeout,
+                                cmd_cb, cb_context);
+}
+
+void digital_poll_next_tech(struct nfc_digital_dev *ddev);
+
+int digital_in_send_sens_req(struct nfc_digital_dev *ddev, u8 rf_tech);
+int digital_in_send_sensf_req(struct nfc_digital_dev *ddev, u8 rf_tech);
+
+int digital_target_found(struct nfc_digital_dev *ddev,
+                         struct nfc_target *target, u8 protocol);
+
+int digital_in_recv_mifare_res(struct sk_buff *resp);
+
+int digital_in_send_atr_req(struct nfc_digital_dev *ddev,
+                            struct nfc_target *target, __u8 comm_mode, __u8 *gb,
+                            size_t gb_len);
+int digital_in_send_dep_req(struct nfc_digital_dev *ddev,
+                            struct nfc_target *target, struct sk_buff *skb,
+                            struct digital_data_exch *data_exch);
+
+int digital_tg_configure_hw(struct nfc_digital_dev *ddev, int type, int param);
+static inline int digital_tg_send_cmd(struct nfc_digital_dev *ddev,
+                        struct sk_buff *skb, u16 timeout,
+                        nfc_digital_cmd_complete_t cmd_cb, void *cb_context)
+{
+        return digital_send_cmd(ddev, DIGITAL_CMD_TG_SEND, skb, NULL, timeout,
+                                cmd_cb, cb_context);
+}
+
+void digital_tg_recv_sens_req(struct nfc_digital_dev *ddev, void *arg,
+                              struct sk_buff *resp);
+
+void digital_tg_recv_sensf_req(struct nfc_digital_dev *ddev, void *arg,
+                               struct sk_buff *resp);
+
+static inline int digital_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
+                                    nfc_digital_cmd_complete_t cb, void *arg)
+{
+        return digital_send_cmd(ddev, DIGITAL_CMD_TG_LISTEN, NULL, NULL,
+                                timeout, cb, arg);
+}
+
+void digital_tg_recv_atr_req(struct nfc_digital_dev *ddev, void *arg,
+                             struct sk_buff *resp);
+
+int digital_tg_send_dep_res(struct nfc_digital_dev *ddev, struct sk_buff *skb);
+
+int digital_tg_listen_nfca(struct nfc_digital_dev *ddev, u8 rf_tech);
+int digital_tg_listen_nfcf(struct nfc_digital_dev *ddev, u8 rf_tech);
+
+typedef u16 (*crc_func_t)(u16, const u8 *, size_t);
+
+#define CRC_A_INIT 0x6363
+#define CRC_B_INIT 0xFFFF
+#define CRC_F_INIT 0x0000
+
+void digital_skb_add_crc(struct sk_buff *skb, crc_func_t crc_func, u16 init,
+                         u8 bitwise_inv, u8 msb_first);
+
+static inline void digital_skb_add_crc_a(struct sk_buff *skb)
+{
+        digital_skb_add_crc(skb, crc_ccitt, CRC_A_INIT, 0, 0);
+}
+
+static inline void digital_skb_add_crc_b(struct sk_buff *skb)
+{
+        digital_skb_add_crc(skb, crc_ccitt, CRC_B_INIT, 1, 0);
+}
+
+static inline void digital_skb_add_crc_f(struct sk_buff *skb)
+{
+        digital_skb_add_crc(skb, crc_itu_t, CRC_F_INIT, 0, 1);
+}
+
+static inline void digital_skb_add_crc_none(struct sk_buff *skb)
+{
+        return;
+}
+
+int digital_skb_check_crc(struct sk_buff *skb, crc_func_t crc_func,
+                          u16 crc_init, u8 bitwise_inv, u8 msb_first);
+
+static inline int digital_skb_check_crc_a(struct sk_buff *skb)
+{
+        return digital_skb_check_crc(skb, crc_ccitt, CRC_A_INIT, 0, 0);
+}
+
+static inline int digital_skb_check_crc_b(struct sk_buff *skb)
+{
+        return digital_skb_check_crc(skb, crc_ccitt, CRC_B_INIT, 1, 0);
+}
+
+static inline int digital_skb_check_crc_f(struct sk_buff *skb)
+{
+        return digital_skb_check_crc(skb, crc_itu_t, CRC_F_INIT, 0, 1);
+}
+
+static inline int digital_skb_check_crc_none(struct sk_buff *skb)
+{
+        return 0;
+}
+
+#endif /* __DIGITAL_H */
diff --git a/net/nfc/digital_core.c b/net/nfc/digital_core.c
new file mode 100644
index 000000000000..09fc95439955
--- /dev/null
+++ b/net/nfc/digital_core.c
@@ -0,0 +1,737 @@
+/*
+ * NFC Digital Protocol stack
+ * Copyright (c) 2013, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ *
+ */
+
+#define pr_fmt(fmt) "digital: %s: " fmt, __func__
+
+#include <linux/module.h>
+
+#include "digital.h"
+
+#define DIGITAL_PROTO_NFCA_RF_TECH \
+        (NFC_PROTO_JEWEL_MASK | NFC_PROTO_MIFARE_MASK | NFC_PROTO_NFC_DEP_MASK)
+
+#define DIGITAL_PROTO_NFCF_RF_TECH \
+        (NFC_PROTO_FELICA_MASK | NFC_PROTO_NFC_DEP_MASK)
+
+struct digital_cmd {
+        struct list_head queue;
+
+        u8 type;
+        u8 pending;
+
+        u16 timeout;
+        struct sk_buff *req;
+        struct sk_buff *resp;
+        struct digital_tg_mdaa_params *mdaa_params;
+
+        nfc_digital_cmd_complete_t cmd_cb;
+        void *cb_context;
+};
+
+struct sk_buff *digital_skb_alloc(struct nfc_digital_dev *ddev,
+                                  unsigned int len)
+{
+        struct sk_buff *skb;
+
+        skb = alloc_skb(len + ddev->tx_headroom + ddev->tx_tailroom,
+                        GFP_KERNEL);
+        if (skb)
+                skb_reserve(skb, ddev->tx_headroom);
+
+        return skb;
+}
+
+void digital_skb_add_crc(struct sk_buff *skb, crc_func_t crc_func, u16 init,
+                         u8 bitwise_inv, u8 msb_first)
+{
+        u16 crc;
+
+        crc = crc_func(init, skb->data, skb->len);
+
+        if (bitwise_inv)
+                crc = ~crc;
+
+        if (msb_first)
+                crc = __fswab16(crc);
+
+        *skb_put(skb, 1) = crc & 0xFF;
+        *skb_put(skb, 1) = (crc >> 8) & 0xFF;
+}
+
+int digital_skb_check_crc(struct sk_buff *skb, crc_func_t crc_func,
+                          u16 crc_init, u8 bitwise_inv, u8 msb_first)
+{
+        int rc;
+        u16 crc;
+
+        if (skb->len <= 2)
+                return -EIO;
+
+        crc = crc_func(crc_init, skb->data, skb->len - 2);
+
+        if (bitwise_inv)
+                crc = ~crc;
+
+        if (msb_first)
+                crc = __swab16(crc);
+
+        rc = (skb->data[skb->len - 2] - (crc & 0xFF)) +
+             (skb->data[skb->len - 1] - ((crc >> 8) & 0xFF));
+
+        if (rc)
+                return -EIO;
+
+        skb_trim(skb, skb->len - 2);
+
+        return 0;
+}
+
+static inline void digital_switch_rf(struct nfc_digital_dev *ddev, bool on)
+{
+        ddev->ops->switch_rf(ddev, on);
+}
+
+static inline void digital_abort_cmd(struct nfc_digital_dev *ddev)
+{
+        ddev->ops->abort_cmd(ddev);
+}
+
+static void digital_wq_cmd_complete(struct work_struct *work)
+{
+        struct digital_cmd *cmd;
+        struct nfc_digital_dev *ddev = container_of(work,
+                                                    struct nfc_digital_dev,
+                                                    cmd_complete_work);
+
+        mutex_lock(&ddev->cmd_lock);
+
+        cmd = list_first_entry_or_null(&ddev->cmd_queue, struct digital_cmd,
+                                       queue);
+        if (!cmd) {
+                mutex_unlock(&ddev->cmd_lock);
+                return;
+        }
+
+        list_del(&cmd->queue);
+
+        mutex_unlock(&ddev->cmd_lock);
+
+        if (!IS_ERR(cmd->resp))
+                print_hex_dump_debug("DIGITAL RX: ", DUMP_PREFIX_NONE, 16, 1,
+                                     cmd->resp->data, cmd->resp->len, false);
+
+        cmd->cmd_cb(ddev, cmd->cb_context, cmd->resp);
+
+        kfree(cmd->mdaa_params);
+        kfree(cmd);
+
+        schedule_work(&ddev->cmd_work);
+}
+
+static void digital_send_cmd_complete(struct nfc_digital_dev *ddev,
+                                      void *arg, struct sk_buff *resp)
+{
+        struct digital_cmd *cmd = arg;
+
+        cmd->resp = resp;
+
+        schedule_work(&ddev->cmd_complete_work);
+}
+
+static void digital_wq_cmd(struct work_struct *work)
+{
+        int rc;
+        struct digital_cmd *cmd;
+        struct digital_tg_mdaa_params *params;
+        struct nfc_digital_dev *ddev = container_of(work,
+                                                    struct nfc_digital_dev,
+                                                    cmd_work);
+
+        mutex_lock(&ddev->cmd_lock);
+
+        cmd = list_first_entry_or_null(&ddev->cmd_queue, struct digital_cmd,
+                                       queue);
+        if (!cmd || cmd->pending) {
+                mutex_unlock(&ddev->cmd_lock);
+                return;
+        }
+
+        mutex_unlock(&ddev->cmd_lock);
+
+        if (cmd->req)
+                print_hex_dump_debug("DIGITAL TX: ", DUMP_PREFIX_NONE, 16, 1,
+                                     cmd->req->data, cmd->req->len, false);
+
+        switch (cmd->type) {
+        case DIGITAL_CMD_IN_SEND:
+                rc = ddev->ops->in_send_cmd(ddev, cmd->req, cmd->timeout,
+                                            digital_send_cmd_complete, cmd);
+                break;
+
+        case DIGITAL_CMD_TG_SEND:
+                rc = ddev->ops->tg_send_cmd(ddev, cmd->req, cmd->timeout,
+                                            digital_send_cmd_complete, cmd);
+                break;
+
+        case DIGITAL_CMD_TG_LISTEN:
+                rc = ddev->ops->tg_listen(ddev, cmd->timeout,
+                                          digital_send_cmd_complete, cmd);
+                break;
+
+        case DIGITAL_CMD_TG_LISTEN_MDAA:
+                params = cmd->mdaa_params;
+
+                rc = ddev->ops->tg_listen_mdaa(ddev, params, cmd->timeout,
+                                               digital_send_cmd_complete, cmd);
+                break;
+
+        default:
+                pr_err("Unknown cmd type %d\n", cmd->type);
+                return;
+        }
+
+        if (!rc)
+                return;
+
+        pr_err("in_send_command returned err %d\n", rc);
+
+        mutex_lock(&ddev->cmd_lock);
+        list_del(&cmd->queue);
+        mutex_unlock(&ddev->cmd_lock);
+
+        kfree_skb(cmd->req);
+        kfree(cmd->mdaa_params);
+        kfree(cmd);
+
+        schedule_work(&ddev->cmd_work);
+}
+
+int digital_send_cmd(struct nfc_digital_dev *ddev, u8 cmd_type,
+                     struct sk_buff *skb, struct digital_tg_mdaa_params *params,
+                     u16 timeout, nfc_digital_cmd_complete_t cmd_cb,
+                     void *cb_context)
+{
+        struct digital_cmd *cmd;
+
+        cmd = kzalloc(sizeof(struct digital_cmd), GFP_KERNEL);
+        if (!cmd)
+                return -ENOMEM;
+
+        cmd->type = cmd_type;
+        cmd->timeout = timeout;
+        cmd->req = skb;
+        cmd->mdaa_params = params;
+        cmd->cmd_cb = cmd_cb;
+        cmd->cb_context = cb_context;
+        INIT_LIST_HEAD(&cmd->queue);
+
+        mutex_lock(&ddev->cmd_lock);
+        list_add_tail(&cmd->queue, &ddev->cmd_queue);
+        mutex_unlock(&ddev->cmd_lock);
+
+        schedule_work(&ddev->cmd_work);
+
+        return 0;
+}
+
+int digital_in_configure_hw(struct nfc_digital_dev *ddev, int type, int param)
+{
+        int rc;
+
+        rc = ddev->ops->in_configure_hw(ddev, type, param);
+        if (rc)
+                pr_err("in_configure_hw failed: %d\n", rc);
+
+        return rc;
+}
+
+int digital_tg_configure_hw(struct nfc_digital_dev *ddev, int type, int param)
+{
+        int rc;
+
+        rc = ddev->ops->tg_configure_hw(ddev, type, param);
+        if (rc)
+                pr_err("tg_configure_hw failed: %d\n", rc);
+
+        return rc;
+}
+
+static int digital_tg_listen_mdaa(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+        struct digital_tg_mdaa_params *params;
+
+        params = kzalloc(sizeof(struct digital_tg_mdaa_params), GFP_KERNEL);
+        if (!params)
+                return -ENOMEM;
+
+        params->sens_res = DIGITAL_SENS_RES_NFC_DEP;
+        get_random_bytes(params->nfcid1, sizeof(params->nfcid1));
+        params->sel_res = DIGITAL_SEL_RES_NFC_DEP;
+
+        params->nfcid2[0] = DIGITAL_SENSF_NFCID2_NFC_DEP_B1;
+        params->nfcid2[1] = DIGITAL_SENSF_NFCID2_NFC_DEP_B2;
+        get_random_bytes(params->nfcid2 + 2, NFC_NFCID2_MAXSIZE - 2);
+        params->sc = DIGITAL_SENSF_FELICA_SC;
+
+        return digital_send_cmd(ddev, DIGITAL_CMD_TG_LISTEN_MDAA, NULL, params,
+                                500, digital_tg_recv_atr_req, NULL);
+}
+
+int digital_target_found(struct nfc_digital_dev *ddev,
+                         struct nfc_target *target, u8 protocol)
+{
+        int rc;
+        u8 framing;
+        u8 rf_tech;
+        int (*check_crc)(struct sk_buff *skb);
+        void (*add_crc)(struct sk_buff *skb);
+
+        rf_tech = ddev->poll_techs[ddev->poll_tech_index].rf_tech;
+
+        switch (protocol) {
+        case NFC_PROTO_JEWEL:
+                framing = NFC_DIGITAL_FRAMING_NFCA_T1T;
+                check_crc = digital_skb_check_crc_b;
+                add_crc = digital_skb_add_crc_b;
+                break;
+
+        case NFC_PROTO_MIFARE:
+                framing = NFC_DIGITAL_FRAMING_NFCA_T2T;
+                check_crc = digital_skb_check_crc_a;
+                add_crc = digital_skb_add_crc_a;
+                break;
+
+        case NFC_PROTO_FELICA:
+                framing = NFC_DIGITAL_FRAMING_NFCF_T3T;
+                check_crc = digital_skb_check_crc_f;
+                add_crc = digital_skb_add_crc_f;
+                break;
+
+        case NFC_PROTO_NFC_DEP:
+                if (rf_tech == NFC_DIGITAL_RF_TECH_106A) {
+                        framing = NFC_DIGITAL_FRAMING_NFCA_NFC_DEP;
+                        check_crc = digital_skb_check_crc_a;
+                        add_crc = digital_skb_add_crc_a;
+                } else {
+                        framing = NFC_DIGITAL_FRAMING_NFCF_NFC_DEP;
+                        check_crc = digital_skb_check_crc_f;
+                        add_crc = digital_skb_add_crc_f;
+                }
+                break;
+
+        default:
+                pr_err("Invalid protocol %d\n", protocol);
+                return -EINVAL;
+        }
+
+        pr_debug("rf_tech=%d, protocol=%d\n", rf_tech, protocol);
+
+        ddev->curr_rf_tech = rf_tech;
+        ddev->curr_protocol = protocol;
+
+        if (DIGITAL_DRV_CAPS_IN_CRC(ddev)) {
+                ddev->skb_add_crc = digital_skb_add_crc_none;
+                ddev->skb_check_crc = digital_skb_check_crc_none;
+        } else {
+                ddev->skb_add_crc = add_crc;
+                ddev->skb_check_crc = check_crc;
+        }
+
+        rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING, framing);
+        if (rc)
+                return rc;
+
+        target->supported_protocols = (1 << protocol);
+        rc = nfc_targets_found(ddev->nfc_dev, target, 1);
+        if (rc)
+                return rc;
+
+        ddev->poll_tech_count = 0;
+
+        return 0;
+}
+
+void digital_poll_next_tech(struct nfc_digital_dev *ddev)
+{
+        digital_switch_rf(ddev, 0);
+
+        mutex_lock(&ddev->poll_lock);
+
+        if (!ddev->poll_tech_count) {
+                mutex_unlock(&ddev->poll_lock);
+                return;
+        }
+
+        ddev->poll_tech_index = (ddev->poll_tech_index + 1) %
+                                ddev->poll_tech_count;
+
+        mutex_unlock(&ddev->poll_lock);
+
+        schedule_work(&ddev->poll_work);
+}
+
+static void digital_wq_poll(struct work_struct *work)
+{
+        int rc;
+        struct digital_poll_tech *poll_tech;
+        struct nfc_digital_dev *ddev = container_of(work,
+                                                    struct nfc_digital_dev,
+                                                    poll_work);
+        mutex_lock(&ddev->poll_lock);
+
+        if (!ddev->poll_tech_count) {
+                mutex_unlock(&ddev->poll_lock);
+                return;
+        }
+
+        poll_tech = &ddev->poll_techs[ddev->poll_tech_index];
+
+        mutex_unlock(&ddev->poll_lock);
+
+        rc = poll_tech->poll_func(ddev, poll_tech->rf_tech);
+        if (rc)
+                digital_poll_next_tech(ddev);
+}
+
+static void digital_add_poll_tech(struct nfc_digital_dev *ddev, u8 rf_tech,
+                                  digital_poll_t poll_func)
+{
+        struct digital_poll_tech *poll_tech;
+
+        if (ddev->poll_tech_count >= NFC_DIGITAL_POLL_MODE_COUNT_MAX)
+                return;
+
+        poll_tech = &ddev->poll_techs[ddev->poll_tech_count++];
+
+        poll_tech->rf_tech = rf_tech;
+        poll_tech->poll_func = poll_func;
+}
+
+/**
+ * start_poll operation
+ *
+ * For every supported protocol, the corresponding polling function is added
+ * to the table of polling technologies (ddev->poll_techs[]) using
+ * digital_add_poll_tech().
+ * When a polling function fails (by timeout or protocol error) the next one is
+ * schedule by digital_poll_next_tech() on the poll workqueue (ddev->poll_work).
+ */
+static int digital_start_poll(struct nfc_dev *nfc_dev, __u32 im_protocols,
+                              __u32 tm_protocols)
+{
+        struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+        u32 matching_im_protocols, matching_tm_protocols;
+
+        pr_debug("protocols: im 0x%x, tm 0x%x, supported 0x%x\n", im_protocols,
+                 tm_protocols, ddev->protocols);
+
+        matching_im_protocols = ddev->protocols & im_protocols;
+        matching_tm_protocols = ddev->protocols & tm_protocols;
+
+        if (!matching_im_protocols && !matching_tm_protocols) {
+                pr_err("Unknown protocol\n");
+                return -EINVAL;
+        }
+
+        if (ddev->poll_tech_count) {
+                pr_err("Already polling\n");
+                return -EBUSY;
+        }
+
+        if (ddev->curr_protocol) {
+                pr_err("A target is already active\n");
+                return -EBUSY;
+        }
+
+        ddev->poll_tech_count = 0;
+        ddev->poll_tech_index = 0;
+
+        if (matching_im_protocols & DIGITAL_PROTO_NFCA_RF_TECH)
+                digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_106A,
+                                      digital_in_send_sens_req);
+
+        if (im_protocols & DIGITAL_PROTO_NFCF_RF_TECH) {
+                digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_212F,
+                                      digital_in_send_sensf_req);
+
+                digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_424F,
+                                      digital_in_send_sensf_req);
+        }
+
+        if (tm_protocols & NFC_PROTO_NFC_DEP_MASK) {
+                if (ddev->ops->tg_listen_mdaa) {
+                        digital_add_poll_tech(ddev, 0,
+                                              digital_tg_listen_mdaa);
+                } else {
+                        digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_106A,
+                                              digital_tg_listen_nfca);
+
+                        digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_212F,
+                                              digital_tg_listen_nfcf);
+
+                        digital_add_poll_tech(ddev, NFC_DIGITAL_RF_TECH_424F,
+                                              digital_tg_listen_nfcf);
+                }
+        }
+
+        if (!ddev->poll_tech_count) {
+                pr_err("Unsupported protocols: im=0x%x, tm=0x%x\n",
+                       matching_im_protocols, matching_tm_protocols);
+                return -EINVAL;
+        }
+
+        schedule_work(&ddev->poll_work);
+
+        return 0;
+}
+
+static void digital_stop_poll(struct nfc_dev *nfc_dev)
+{
+        struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+        mutex_lock(&ddev->poll_lock);
+
+        if (!ddev->poll_tech_count) {
+                pr_err("Polling operation was not running\n");
+                mutex_unlock(&ddev->poll_lock);
+                return;
+        }
+
+        ddev->poll_tech_count = 0;
+
+        mutex_unlock(&ddev->poll_lock);
+
+        cancel_work_sync(&ddev->poll_work);
+
+        digital_abort_cmd(ddev);
+}
+
+static int digital_dev_up(struct nfc_dev *nfc_dev)
+{
+        struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+        digital_switch_rf(ddev, 1);
+
+        return 0;
+}
+
+static int digital_dev_down(struct nfc_dev *nfc_dev)
+{
+        struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+        digital_switch_rf(ddev, 0);
+
+        return 0;
+}
+
+static int digital_dep_link_up(struct nfc_dev *nfc_dev,
+                               struct nfc_target *target,
+                               __u8 comm_mode, __u8 *gb, size_t gb_len)
+{
+        struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+        return digital_in_send_atr_req(ddev, target, comm_mode, gb, gb_len);
+}
+
+static int digital_dep_link_down(struct nfc_dev *nfc_dev)
+{
+        struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+        ddev->curr_protocol = 0;
+
+        return 0;
+}
+
+static int digital_activate_target(struct nfc_dev *nfc_dev,
+                                   struct nfc_target *target, __u32 protocol)
+{
+        return 0;
+}
+
+static void digital_deactivate_target(struct nfc_dev *nfc_dev,
+                                      struct nfc_target *target)
+{
+        struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+
+        ddev->curr_protocol = 0;
+}
+
+static int digital_tg_send(struct nfc_dev *dev, struct sk_buff *skb)
+{
+        struct nfc_digital_dev *ddev = nfc_get_drvdata(dev);
+
+        return digital_tg_send_dep_res(ddev, skb);
+}
+
+static void digital_in_send_complete(struct nfc_digital_dev *ddev, void *arg,
+                                     struct sk_buff *resp)
+{
+        struct digital_data_exch *data_exch = arg;
+        int rc;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                goto done;
+        }
+
+        if (ddev->curr_protocol == NFC_PROTO_MIFARE)
+                rc = digital_in_recv_mifare_res(resp);
+        else
+                rc = ddev->skb_check_crc(resp);
+
+        if (rc) {
+                kfree_skb(resp);
+                resp = NULL;
+        }
+
+done:
+        data_exch->cb(data_exch->cb_context, resp, rc);
+
+        kfree(data_exch);
+}
+
+static int digital_in_send(struct nfc_dev *nfc_dev, struct nfc_target *target,
+                           struct sk_buff *skb, data_exchange_cb_t cb,
+                           void *cb_context)
+{
+        struct nfc_digital_dev *ddev = nfc_get_drvdata(nfc_dev);
+        struct digital_data_exch *data_exch;
+
+        data_exch = kzalloc(sizeof(struct digital_data_exch), GFP_KERNEL);
+        if (!data_exch) {
+                pr_err("Failed to allocate data_exch struct\n");
+                return -ENOMEM;
+        }
+
+        data_exch->cb = cb;
+        data_exch->cb_context = cb_context;
+
+        if (ddev->curr_protocol == NFC_PROTO_NFC_DEP)
+                return digital_in_send_dep_req(ddev, target, skb, data_exch);
+
+        ddev->skb_add_crc(skb);
+
+        return digital_in_send_cmd(ddev, skb, 500, digital_in_send_complete,
+                                   data_exch);
+}
+
+static struct nfc_ops digital_nfc_ops = {
+        .dev_up = digital_dev_up,
+        .dev_down = digital_dev_down,
+        .start_poll = digital_start_poll,
+        .stop_poll = digital_stop_poll,
+        .dep_link_up = digital_dep_link_up,
+        .dep_link_down = digital_dep_link_down,
+        .activate_target = digital_activate_target,
+        .deactivate_target = digital_deactivate_target,
+        .tm_send = digital_tg_send,
+        .im_transceive = digital_in_send,
+};
+
+struct nfc_digital_dev *nfc_digital_allocate_device(struct nfc_digital_ops *ops,
+                                            __u32 supported_protocols,
+                                            __u32 driver_capabilities,
+                                            int tx_headroom, int tx_tailroom)
+{
+        struct nfc_digital_dev *ddev;
+
+        if (!ops->in_configure_hw || !ops->in_send_cmd || !ops->tg_listen ||
+            !ops->tg_configure_hw || !ops->tg_send_cmd || !ops->abort_cmd ||
+            !ops->switch_rf)
+                return NULL;
+
+        ddev = kzalloc(sizeof(struct nfc_digital_dev), GFP_KERNEL);
+        if (!ddev)
+                return NULL;
+
+        ddev->driver_capabilities = driver_capabilities;
+        ddev->ops = ops;
+
+        mutex_init(&ddev->cmd_lock);
+        INIT_LIST_HEAD(&ddev->cmd_queue);
+
+        INIT_WORK(&ddev->cmd_work, digital_wq_cmd);
+        INIT_WORK(&ddev->cmd_complete_work, digital_wq_cmd_complete);
+
+        mutex_init(&ddev->poll_lock);
+        INIT_WORK(&ddev->poll_work, digital_wq_poll);
+
+        if (supported_protocols & NFC_PROTO_JEWEL_MASK)
+                ddev->protocols |= NFC_PROTO_JEWEL_MASK;
+        if (supported_protocols & NFC_PROTO_MIFARE_MASK)
+                ddev->protocols |= NFC_PROTO_MIFARE_MASK;
+        if (supported_protocols & NFC_PROTO_FELICA_MASK)
+                ddev->protocols |= NFC_PROTO_FELICA_MASK;
+        if (supported_protocols & NFC_PROTO_NFC_DEP_MASK)
+                ddev->protocols |= NFC_PROTO_NFC_DEP_MASK;
+
+        ddev->tx_headroom = tx_headroom + DIGITAL_MAX_HEADER_LEN;
+        ddev->tx_tailroom = tx_tailroom + DIGITAL_CRC_LEN;
+
+        ddev->nfc_dev = nfc_allocate_device(&digital_nfc_ops, ddev->protocols,
+                                            ddev->tx_headroom,
+                                            ddev->tx_tailroom);
+        if (!ddev->nfc_dev) {
+                pr_err("nfc_allocate_device failed\n");
+                goto free_dev;
+        }
+
+        nfc_set_drvdata(ddev->nfc_dev, ddev);
+
+        return ddev;
+
+free_dev:
+        kfree(ddev);
+
+        return NULL;
+}
+EXPORT_SYMBOL(nfc_digital_allocate_device);
+
+void nfc_digital_free_device(struct nfc_digital_dev *ddev)
+{
+        nfc_free_device(ddev->nfc_dev);
+        kfree(ddev);
+}
+EXPORT_SYMBOL(nfc_digital_free_device);
+
+int nfc_digital_register_device(struct nfc_digital_dev *ddev)
+{
+        return nfc_register_device(ddev->nfc_dev);
+}
+EXPORT_SYMBOL(nfc_digital_register_device);
+
+void nfc_digital_unregister_device(struct nfc_digital_dev *ddev)
+{
+        struct digital_cmd *cmd, *n;
+
+        nfc_unregister_device(ddev->nfc_dev);
+
+        mutex_lock(&ddev->poll_lock);
+        ddev->poll_tech_count = 0;
+        mutex_unlock(&ddev->poll_lock);
+
+        cancel_work_sync(&ddev->poll_work);
+        cancel_work_sync(&ddev->cmd_work);
+        cancel_work_sync(&ddev->cmd_complete_work);
+
+        list_for_each_entry_safe(cmd, n, &ddev->cmd_queue, queue) {
+                list_del(&cmd->queue);
+                kfree(cmd->mdaa_params);
+                kfree(cmd);
+        }
+}
+EXPORT_SYMBOL(nfc_digital_unregister_device);
+
+MODULE_LICENSE("GPL");
diff --git a/net/nfc/digital_dep.c b/net/nfc/digital_dep.c
new file mode 100644
index 000000000000..07bbc24fb4c7
--- /dev/null
+++ b/net/nfc/digital_dep.c
@@ -0,0 +1,729 @@
+/*
+ * NFC Digital Protocol stack
+ * Copyright (c) 2013, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ *
+ */
+
+#define pr_fmt(fmt) "digital: %s: " fmt, __func__
+
+#include "digital.h"
+
+#define DIGITAL_NFC_DEP_FRAME_DIR_OUT 0xD4
+#define DIGITAL_NFC_DEP_FRAME_DIR_IN  0xD5
+
+#define DIGITAL_NFC_DEP_NFCA_SOD_SB   0xF0
+
+#define DIGITAL_CMD_ATR_REQ 0x00
+#define DIGITAL_CMD_ATR_RES 0x01
+#define DIGITAL_CMD_PSL_REQ 0x04
+#define DIGITAL_CMD_PSL_RES 0x05
+#define DIGITAL_CMD_DEP_REQ 0x06
+#define DIGITAL_CMD_DEP_RES 0x07
+
+#define DIGITAL_ATR_REQ_MIN_SIZE 16
+#define DIGITAL_ATR_REQ_MAX_SIZE 64
+
+#define DIGITAL_NFCID3_LEN ((u8)8)
+#define DIGITAL_LR_BITS_PAYLOAD_SIZE_254B 0x30
+#define DIGITAL_GB_BIT  0x02
+
+#define DIGITAL_NFC_DEP_PFB_TYPE(pfb) ((pfb) & 0xE0)
+
+#define DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT 0x10
+
+#define DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb) \
+                                ((pfb) & DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT)
+#define DIGITAL_NFC_DEP_MI_BIT_SET(pfb)  ((pfb) & 0x10)
+#define DIGITAL_NFC_DEP_NAD_BIT_SET(pfb) ((pfb) & 0x08)
+#define DIGITAL_NFC_DEP_DID_BIT_SET(pfb) ((pfb) & 0x04)
+#define DIGITAL_NFC_DEP_PFB_PNI(pfb)     ((pfb) & 0x03)
+
+#define DIGITAL_NFC_DEP_PFB_I_PDU          0x00
+#define DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU   0x40
+#define DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU 0x80
+
+struct digital_atr_req {
+        u8 dir;
+        u8 cmd;
+        u8 nfcid3[10];
+        u8 did;
+        u8 bs;
+        u8 br;
+        u8 pp;
+        u8 gb[0];
+} __packed;
+
+struct digital_atr_res {
+        u8 dir;
+        u8 cmd;
+        u8 nfcid3[10];
+        u8 did;
+        u8 bs;
+        u8 br;
+        u8 to;
+        u8 pp;
+        u8 gb[0];
+} __packed;
+
+struct digital_psl_req {
+        u8 dir;
+        u8 cmd;
+        u8 did;
+        u8 brs;
+        u8 fsl;
+} __packed;
+
+struct digital_psl_res {
+        u8 dir;
+        u8 cmd;
+        u8 did;
+} __packed;
+
+struct digital_dep_req_res {
+        u8 dir;
+        u8 cmd;
+        u8 pfb;
+} __packed;
+
+static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg,
+                                    struct sk_buff *resp);
+
+static void digital_skb_push_dep_sod(struct nfc_digital_dev *ddev,
+                                     struct sk_buff *skb)
+{
+        skb_push(skb, sizeof(u8));
+
+        skb->data[0] = skb->len;
+
+        if (ddev->curr_rf_tech == NFC_DIGITAL_RF_TECH_106A)
+                *skb_push(skb, sizeof(u8)) = DIGITAL_NFC_DEP_NFCA_SOD_SB;
+}
+
+static int digital_skb_pull_dep_sod(struct nfc_digital_dev *ddev,
+                                    struct sk_buff *skb)
+{
+        u8 size;
+
+        if (skb->len < 2)
+                return -EIO;
+
+        if (ddev->curr_rf_tech == NFC_DIGITAL_RF_TECH_106A)
+                skb_pull(skb, sizeof(u8));
+
+        size = skb->data[0];
+        if (size != skb->len)
+                return -EIO;
+
+        skb_pull(skb, sizeof(u8));
+
+        return 0;
+}
+
+static void digital_in_recv_atr_res(struct nfc_digital_dev *ddev, void *arg,
+                                 struct sk_buff *resp)
+{
+        struct nfc_target *target = arg;
+        struct digital_atr_res *atr_res;
+        u8 gb_len;
+        int rc;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        rc = ddev->skb_check_crc(resp);
+        if (rc) {
+                PROTOCOL_ERR("14.4.1.6");
+                goto exit;
+        }
+
+        rc = digital_skb_pull_dep_sod(ddev, resp);
+        if (rc) {
+                PROTOCOL_ERR("14.4.1.2");
+                goto exit;
+        }
+
+        if (resp->len < sizeof(struct digital_atr_res)) {
+                rc = -EIO;
+                goto exit;
+        }
+
+        gb_len = resp->len - sizeof(struct digital_atr_res);
+
+        atr_res = (struct digital_atr_res *)resp->data;
+
+        rc = nfc_set_remote_general_bytes(ddev->nfc_dev, atr_res->gb, gb_len);
+        if (rc)
+                goto exit;
+
+        rc = nfc_dep_link_is_up(ddev->nfc_dev, target->idx, NFC_COMM_ACTIVE,
+                                NFC_RF_INITIATOR);
+
+        ddev->curr_nfc_dep_pni = 0;
+
+exit:
+        dev_kfree_skb(resp);
+
+        if (rc)
+                ddev->curr_protocol = 0;
+}
+
+int digital_in_send_atr_req(struct nfc_digital_dev *ddev,
+                            struct nfc_target *target, __u8 comm_mode, __u8 *gb,
+                            size_t gb_len)
+{
+        struct sk_buff *skb;
+        struct digital_atr_req *atr_req;
+        uint size;
+
+        size = DIGITAL_ATR_REQ_MIN_SIZE + gb_len;
+
+        if (size > DIGITAL_ATR_REQ_MAX_SIZE) {
+                PROTOCOL_ERR("14.6.1.1");
+                return -EINVAL;
+        }
+
+        skb = digital_skb_alloc(ddev, size);
+        if (!skb)
+                return -ENOMEM;
+
+        skb_put(skb, sizeof(struct digital_atr_req));
+
+        atr_req = (struct digital_atr_req *)skb->data;
+        memset(atr_req, 0, sizeof(struct digital_atr_req));
+
+        atr_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
+        atr_req->cmd = DIGITAL_CMD_ATR_REQ;
+        if (target->nfcid2_len)
+                memcpy(atr_req->nfcid3, target->nfcid2,
+                       max(target->nfcid2_len, DIGITAL_NFCID3_LEN));
+        else
+                get_random_bytes(atr_req->nfcid3, DIGITAL_NFCID3_LEN);
+
+        atr_req->did = 0;
+        atr_req->bs = 0;
+        atr_req->br = 0;
+
+        atr_req->pp = DIGITAL_LR_BITS_PAYLOAD_SIZE_254B;
+
+        if (gb_len) {
+                atr_req->pp |= DIGITAL_GB_BIT;
+                memcpy(skb_put(skb, gb_len), gb, gb_len);
+        }
+
+        digital_skb_push_dep_sod(ddev, skb);
+
+        ddev->skb_add_crc(skb);
+
+        digital_in_send_cmd(ddev, skb, 500, digital_in_recv_atr_res, target);
+
+        return 0;
+}
+
+static int digital_in_send_rtox(struct nfc_digital_dev *ddev,
+                                struct digital_data_exch *data_exch, u8 rtox)
+{
+        struct digital_dep_req_res *dep_req;
+        struct sk_buff *skb;
+        int rc;
+
+        skb = digital_skb_alloc(ddev, 1);
+        if (!skb)
+                return -ENOMEM;
+
+        *skb_put(skb, 1) = rtox;
+
+        skb_push(skb, sizeof(struct digital_dep_req_res));
+
+        dep_req = (struct digital_dep_req_res *)skb->data;
+
+        dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
+        dep_req->cmd = DIGITAL_CMD_DEP_REQ;
+        dep_req->pfb = DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU |
+                       DIGITAL_NFC_DEP_PFB_TIMEOUT_BIT;
+
+        digital_skb_push_dep_sod(ddev, skb);
+
+        ddev->skb_add_crc(skb);
+
+        rc = digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
+                                 data_exch);
+
+        return rc;
+}
+
+static void digital_in_recv_dep_res(struct nfc_digital_dev *ddev, void *arg,
+                                    struct sk_buff *resp)
+{
+        struct digital_data_exch *data_exch = arg;
+        struct digital_dep_req_res *dep_res;
+        u8 pfb;
+        uint size;
+        int rc;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        rc = ddev->skb_check_crc(resp);
+        if (rc) {
+                PROTOCOL_ERR("14.4.1.6");
+                goto error;
+        }
+
+        rc = digital_skb_pull_dep_sod(ddev, resp);
+        if (rc) {
+                PROTOCOL_ERR("14.4.1.2");
+                goto exit;
+        }
+
+        dep_res = (struct digital_dep_req_res *)resp->data;
+
+        if (resp->len < sizeof(struct digital_dep_req_res) ||
+            dep_res->dir != DIGITAL_NFC_DEP_FRAME_DIR_IN ||
+            dep_res->cmd != DIGITAL_CMD_DEP_RES) {
+                rc = -EIO;
+                goto error;
+        }
+
+        pfb = dep_res->pfb;
+
+        switch (DIGITAL_NFC_DEP_PFB_TYPE(pfb)) {
+        case DIGITAL_NFC_DEP_PFB_I_PDU:
+                if (DIGITAL_NFC_DEP_PFB_PNI(pfb) != ddev->curr_nfc_dep_pni) {
+                        PROTOCOL_ERR("14.12.3.3");
+                        rc = -EIO;
+                        goto error;
+                }
+
+                ddev->curr_nfc_dep_pni =
+                        DIGITAL_NFC_DEP_PFB_PNI(ddev->curr_nfc_dep_pni + 1);
+                rc = 0;
+                break;
+
+        case DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU:
+                pr_err("Received a ACK/NACK PDU\n");
+                rc = -EIO;
+                goto error;
+
+        case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU:
+                if (!DIGITAL_NFC_DEP_PFB_IS_TIMEOUT(pfb)) {
+                        rc = -EINVAL;
+                        goto error;
+                }
+
+                rc = digital_in_send_rtox(ddev, data_exch, resp->data[3]);
+                if (rc)
+                        goto error;
+
+                kfree_skb(resp);
+                return;
+        }
+
+        if (DIGITAL_NFC_DEP_MI_BIT_SET(pfb)) {
+                pr_err("MI bit set. Chained PDU not supported\n");
+                rc = -EIO;
+                goto error;
+        }
+
+        size = sizeof(struct digital_dep_req_res);
+
+        if (DIGITAL_NFC_DEP_DID_BIT_SET(pfb))
+                size++;
+
+        if (size > resp->len) {
+                rc = -EIO;
+                goto error;
+        }
+
+        skb_pull(resp, size);
+
+exit:
+        data_exch->cb(data_exch->cb_context, resp, rc);
+
+error:
+        kfree(data_exch);
+
+        if (rc)
+                kfree_skb(resp);
+}
+
+int digital_in_send_dep_req(struct nfc_digital_dev *ddev,
+                            struct nfc_target *target, struct sk_buff *skb,
+                            struct digital_data_exch *data_exch)
+{
+        struct digital_dep_req_res *dep_req;
+
+        skb_push(skb, sizeof(struct digital_dep_req_res));
+
+        dep_req = (struct digital_dep_req_res *)skb->data;
+        dep_req->dir = DIGITAL_NFC_DEP_FRAME_DIR_OUT;
+        dep_req->cmd = DIGITAL_CMD_DEP_REQ;
+        dep_req->pfb = ddev->curr_nfc_dep_pni;
+
+        digital_skb_push_dep_sod(ddev, skb);
+
+        ddev->skb_add_crc(skb);
+
+        return digital_in_send_cmd(ddev, skb, 1500, digital_in_recv_dep_res,
+                                   data_exch);
+}
+
+static void digital_tg_recv_dep_req(struct nfc_digital_dev *ddev, void *arg,
+                                    struct sk_buff *resp)
+{
+        int rc;
+        struct digital_dep_req_res *dep_req;
+        size_t size;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        rc = ddev->skb_check_crc(resp);
+        if (rc) {
+                PROTOCOL_ERR("14.4.1.6");
+                goto exit;
+        }
+
+        rc = digital_skb_pull_dep_sod(ddev, resp);
+        if (rc) {
+                PROTOCOL_ERR("14.4.1.2");
+                goto exit;
+        }
+
+        size = sizeof(struct digital_dep_req_res);
+        dep_req = (struct digital_dep_req_res *)resp->data;
+
+        if (resp->len < size || dep_req->dir != DIGITAL_NFC_DEP_FRAME_DIR_OUT ||
+            dep_req->cmd != DIGITAL_CMD_DEP_REQ) {
+                rc = -EIO;
+                goto exit;
+        }
+
+        if (DIGITAL_NFC_DEP_DID_BIT_SET(dep_req->pfb))
+                size++;
+
+        if (resp->len < size) {
+                rc = -EIO;
+                goto exit;
+        }
+
+        switch (DIGITAL_NFC_DEP_PFB_TYPE(dep_req->pfb)) {
+        case DIGITAL_NFC_DEP_PFB_I_PDU:
+                pr_debug("DIGITAL_NFC_DEP_PFB_I_PDU\n");
+                ddev->curr_nfc_dep_pni = DIGITAL_NFC_DEP_PFB_PNI(dep_req->pfb);
+                break;
+        case DIGITAL_NFC_DEP_PFB_ACK_NACK_PDU:
+                pr_err("Received a ACK/NACK PDU\n");
+                rc = -EINVAL;
+                goto exit;
+                break;
+        case DIGITAL_NFC_DEP_PFB_SUPERVISOR_PDU:
+                pr_err("Received a SUPERVISOR PDU\n");
+                rc = -EINVAL;
+                goto exit;
+                break;
+        }
+
+        skb_pull(resp, size);
+
+        rc = nfc_tm_data_received(ddev->nfc_dev, resp);
+
+exit:
+        if (rc)
+                kfree_skb(resp);
+}
+
+int digital_tg_send_dep_res(struct nfc_digital_dev *ddev, struct sk_buff *skb)
+{
+        struct digital_dep_req_res *dep_res;
+
+        skb_push(skb, sizeof(struct digital_dep_req_res));
+        dep_res = (struct digital_dep_req_res *)skb->data;
+
+        dep_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN;
+        dep_res->cmd = DIGITAL_CMD_DEP_RES;
+        dep_res->pfb = ddev->curr_nfc_dep_pni;
+
+        digital_skb_push_dep_sod(ddev, skb);
+
+        ddev->skb_add_crc(skb);
+
+        return digital_tg_send_cmd(ddev, skb, 1500, digital_tg_recv_dep_req,
+                                   NULL);
+}
+
+static void digital_tg_send_psl_res_complete(struct nfc_digital_dev *ddev,
+                                             void *arg, struct sk_buff *resp)
+{
+        u8 rf_tech = PTR_ERR(arg);
+
+        if (IS_ERR(resp))
+                return;
+
+        digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH, rf_tech);
+
+        digital_tg_listen(ddev, 1500, digital_tg_recv_dep_req, NULL);
+
+        dev_kfree_skb(resp);
+}
+
+static int digital_tg_send_psl_res(struct nfc_digital_dev *ddev, u8 did,
+                                   u8 rf_tech)
+{
+        struct digital_psl_res *psl_res;
+        struct sk_buff *skb;
+        int rc;
+
+        skb = digital_skb_alloc(ddev, sizeof(struct digital_psl_res));
+        if (!skb)
+                return -ENOMEM;
+
+        skb_put(skb, sizeof(struct digital_psl_res));
+
+        psl_res = (struct digital_psl_res *)skb->data;
+
+        psl_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN;
+        psl_res->cmd = DIGITAL_CMD_PSL_RES;
+        psl_res->did = did;
+
+        digital_skb_push_dep_sod(ddev, skb);
+
+        ddev->skb_add_crc(skb);
+
+        rc = digital_tg_send_cmd(ddev, skb, 0, digital_tg_send_psl_res_complete,
+                                 ERR_PTR(rf_tech));
+
+        if (rc)
+                kfree_skb(skb);
+
+        return rc;
+}
+
+static void digital_tg_recv_psl_req(struct nfc_digital_dev *ddev, void *arg,
+                                    struct sk_buff *resp)
+{
+        int rc;
+        struct digital_psl_req *psl_req;
+        u8 rf_tech;
+        u8 dsi;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        rc = ddev->skb_check_crc(resp);
+        if (rc) {
+                PROTOCOL_ERR("14.4.1.6");
+                goto exit;
+        }
+
+        rc = digital_skb_pull_dep_sod(ddev, resp);
+        if (rc) {
+                PROTOCOL_ERR("14.4.1.2");
+                goto exit;
+        }
+
+        psl_req = (struct digital_psl_req *)resp->data;
+
+        if (resp->len != sizeof(struct digital_psl_req) ||
+            psl_req->dir != DIGITAL_NFC_DEP_FRAME_DIR_OUT ||
+            psl_req->cmd != DIGITAL_CMD_PSL_REQ) {
+                rc = -EIO;
+                goto exit;
+        }
+
+        dsi = (psl_req->brs >> 3) & 0x07;
+        switch (dsi) {
+        case 0:
+                rf_tech = NFC_DIGITAL_RF_TECH_106A;
+                break;
+        case 1:
+                rf_tech = NFC_DIGITAL_RF_TECH_212F;
+                break;
+        case 2:
+                rf_tech = NFC_DIGITAL_RF_TECH_424F;
+                break;
+        default:
+                pr_err("Unsuported dsi value %d\n", dsi);
+                goto exit;
+        }
+
+        rc = digital_tg_send_psl_res(ddev, psl_req->did, rf_tech);
+
+exit:
+        kfree_skb(resp);
+}
+
+static void digital_tg_send_atr_res_complete(struct nfc_digital_dev *ddev,
+                                             void *arg, struct sk_buff *resp)
+{
+        int offset;
+
+        if (IS_ERR(resp)) {
+                digital_poll_next_tech(ddev);
+                return;
+        }
+
+        offset = 2;
+        if (resp->data[0] == DIGITAL_NFC_DEP_NFCA_SOD_SB)
+                offset++;
+
+        if (resp->data[offset] == DIGITAL_CMD_PSL_REQ)
+                digital_tg_recv_psl_req(ddev, arg, resp);
+        else
+                digital_tg_recv_dep_req(ddev, arg, resp);
+}
+
+static int digital_tg_send_atr_res(struct nfc_digital_dev *ddev,
+                                   struct digital_atr_req *atr_req)
+{
+        struct digital_atr_res *atr_res;
+        struct sk_buff *skb;
+        u8 *gb;
+        size_t gb_len;
+        int rc;
+
+        gb = nfc_get_local_general_bytes(ddev->nfc_dev, &gb_len);
+        if (!gb)
+                gb_len = 0;
+
+        skb = digital_skb_alloc(ddev, sizeof(struct digital_atr_res) + gb_len);
+        if (!skb)
+                return -ENOMEM;
+
+        skb_put(skb, sizeof(struct digital_atr_res));
+        atr_res = (struct digital_atr_res *)skb->data;
+
+        memset(atr_res, 0, sizeof(struct digital_atr_res));
+
+        atr_res->dir = DIGITAL_NFC_DEP_FRAME_DIR_IN;
+        atr_res->cmd = DIGITAL_CMD_ATR_RES;
+        memcpy(atr_res->nfcid3, atr_req->nfcid3, sizeof(atr_req->nfcid3));
+        atr_res->to = 8;
+        atr_res->pp = DIGITAL_LR_BITS_PAYLOAD_SIZE_254B;
+        if (gb_len) {
+                skb_put(skb, gb_len);
+
+                atr_res->pp |= DIGITAL_GB_BIT;
+                memcpy(atr_res->gb, gb, gb_len);
+        }
+
+        digital_skb_push_dep_sod(ddev, skb);
+
+        ddev->skb_add_crc(skb);
+
+        rc = digital_tg_send_cmd(ddev, skb, 999,
+                                 digital_tg_send_atr_res_complete, NULL);
+        if (rc) {
+                kfree_skb(skb);
+                return rc;
+        }
+
+        return rc;
+}
+
+void digital_tg_recv_atr_req(struct nfc_digital_dev *ddev, void *arg,
+                             struct sk_buff *resp)
+{
+        int rc;
+        struct digital_atr_req *atr_req;
+        size_t gb_len, min_size;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        if (!resp->len) {
+                rc = -EIO;
+                goto exit;
+        }
+
+        if (resp->data[0] == DIGITAL_NFC_DEP_NFCA_SOD_SB) {
+                min_size = DIGITAL_ATR_REQ_MIN_SIZE + 2;
+
+                ddev->curr_rf_tech = NFC_DIGITAL_RF_TECH_106A;
+                ddev->skb_add_crc = digital_skb_add_crc_a;
+                ddev->skb_check_crc = digital_skb_check_crc_a;
+        } else {
+                min_size = DIGITAL_ATR_REQ_MIN_SIZE + 1;
+
+                ddev->curr_rf_tech = NFC_DIGITAL_RF_TECH_212F;
+                ddev->skb_add_crc = digital_skb_add_crc_f;
+                ddev->skb_check_crc = digital_skb_check_crc_f;
+        }
+
+        if (resp->len < min_size) {
+                rc = -EIO;
+                goto exit;
+        }
+
+        if (DIGITAL_DRV_CAPS_TG_CRC(ddev)) {
+                ddev->skb_add_crc = digital_skb_add_crc_none;
+                ddev->skb_check_crc = digital_skb_check_crc_none;
+        }
+
+        rc = ddev->skb_check_crc(resp);
+        if (rc) {
+                PROTOCOL_ERR("14.4.1.6");
+                goto exit;
+        }
+
+        rc = digital_skb_pull_dep_sod(ddev, resp);
+        if (rc) {
+                PROTOCOL_ERR("14.4.1.2");
+                goto exit;
+        }
+
+        atr_req = (struct digital_atr_req *)resp->data;
+
+        if (atr_req->dir != DIGITAL_NFC_DEP_FRAME_DIR_OUT ||
+            atr_req->cmd != DIGITAL_CMD_ATR_REQ) {
+                rc = -EINVAL;
+                goto exit;
+        }
+
+        rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+                                     NFC_DIGITAL_FRAMING_NFC_DEP_ACTIVATED);
+        if (rc)
+                goto exit;
+
+        rc = digital_tg_send_atr_res(ddev, atr_req);
+        if (rc)
+                goto exit;
+
+        gb_len = resp->len - sizeof(struct digital_atr_req);
+        rc = nfc_tm_activated(ddev->nfc_dev, NFC_PROTO_NFC_DEP_MASK,
+                              NFC_COMM_PASSIVE, atr_req->gb, gb_len);
+        if (rc)
+                goto exit;
+
+        ddev->poll_tech_count = 0;
+
+        rc = 0;
+exit:
+        if (rc)
+                digital_poll_next_tech(ddev);
+
+        dev_kfree_skb(resp);
+}
diff --git a/net/nfc/digital_technology.c b/net/nfc/digital_technology.c
new file mode 100644
index 000000000000..251c8c753ebe
--- /dev/null
+++ b/net/nfc/digital_technology.c
@@ -0,0 +1,770 @@
+/*
+ * NFC Digital Protocol stack
+ * Copyright (c) 2013, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope 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.
+ *
+ */
+
+#define pr_fmt(fmt) "digital: %s: " fmt, __func__
+
+#include "digital.h"
+
+#define DIGITAL_CMD_SENS_REQ    0x26
+#define DIGITAL_CMD_ALL_REQ     0x52
+#define DIGITAL_CMD_SEL_REQ_CL1 0x93
+#define DIGITAL_CMD_SEL_REQ_CL2 0x95
+#define DIGITAL_CMD_SEL_REQ_CL3 0x97
+
+#define DIGITAL_SDD_REQ_SEL_PAR 0x20
+
+#define DIGITAL_SDD_RES_CT  0x88
+#define DIGITAL_SDD_RES_LEN 5
+
+#define DIGITAL_SEL_RES_NFCID1_COMPLETE(sel_res) (!((sel_res) & 0x04))
+#define DIGITAL_SEL_RES_IS_T2T(sel_res) (!((sel_res) & 0x60))
+#define DIGITAL_SEL_RES_IS_NFC_DEP(sel_res) ((sel_res) & 0x40)
+
+#define DIGITAL_SENS_RES_IS_T1T(sens_res) (((sens_res) & 0x0C00) == 0x0C00)
+#define DIGITAL_SENS_RES_IS_VALID(sens_res) \
+        ((!((sens_res) & 0x001F) && (((sens_res) & 0x0C00) == 0x0C00)) || \
+        (((sens_res) & 0x001F) && ((sens_res) & 0x0C00) != 0x0C00))
+
+#define DIGITAL_MIFARE_READ_RES_LEN 16
+#define DIGITAL_MIFARE_ACK_RES  0x0A
+
+#define DIGITAL_CMD_SENSF_REQ   0x00
+#define DIGITAL_CMD_SENSF_RES   0x01
+
+#define DIGITAL_SENSF_RES_MIN_LENGTH 17
+#define DIGITAL_SENSF_RES_RD_AP_B1   0x00
+#define DIGITAL_SENSF_RES_RD_AP_B2   0x8F
+
+#define DIGITAL_SENSF_REQ_RC_NONE 0
+#define DIGITAL_SENSF_REQ_RC_SC   1
+#define DIGITAL_SENSF_REQ_RC_AP   2
+
+struct digital_sdd_res {
+        u8 nfcid1[4];
+        u8 bcc;
+} __packed;
+
+struct digital_sel_req {
+        u8 sel_cmd;
+        u8 b2;
+        u8 nfcid1[4];
+        u8 bcc;
+} __packed;
+
+struct digital_sensf_req {
+        u8 cmd;
+        u8 sc1;
+        u8 sc2;
+        u8 rc;
+        u8 tsn;
+} __packed;
+
+struct digital_sensf_res {
+        u8 cmd;
+        u8 nfcid2[8];
+        u8 pad0[2];
+        u8 pad1[3];
+        u8 mrti_check;
+        u8 mrti_update;
+        u8 pad2;
+        u8 rd[2];
+} __packed;
+
+static int digital_in_send_sdd_req(struct nfc_digital_dev *ddev,
+                                   struct nfc_target *target);
+
+static void digital_in_recv_sel_res(struct nfc_digital_dev *ddev, void *arg,
+                                    struct sk_buff *resp)
+{
+        struct nfc_target *target = arg;
+        int rc;
+        u8 sel_res;
+        u8 nfc_proto;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        if (!DIGITAL_DRV_CAPS_IN_CRC(ddev)) {
+                rc = digital_skb_check_crc_a(resp);
+                if (rc) {
+                        PROTOCOL_ERR("4.4.1.3");
+                        goto exit;
+                }
+        }
+
+        if (!resp->len) {
+                rc = -EIO;
+                goto exit;
+        }
+
+        sel_res = resp->data[0];
+
+        if (!DIGITAL_SEL_RES_NFCID1_COMPLETE(sel_res)) {
+                rc = digital_in_send_sdd_req(ddev, target);
+                if (rc)
+                        goto exit;
+
+                goto exit_free_skb;
+        }
+
+        if (DIGITAL_SEL_RES_IS_T2T(sel_res)) {
+                nfc_proto = NFC_PROTO_MIFARE;
+        } else if (DIGITAL_SEL_RES_IS_NFC_DEP(sel_res)) {
+                nfc_proto = NFC_PROTO_NFC_DEP;
+        } else {
+                rc = -EOPNOTSUPP;
+                goto exit;
+        }
+
+        target->sel_res = sel_res;
+
+        rc = digital_target_found(ddev, target, nfc_proto);
+
+exit:
+        kfree(target);
+
+exit_free_skb:
+        dev_kfree_skb(resp);
+
+        if (rc)
+                digital_poll_next_tech(ddev);
+}
+
+static int digital_in_send_sel_req(struct nfc_digital_dev *ddev,
+                                   struct nfc_target *target,
+                                   struct digital_sdd_res *sdd_res)
+{
+        struct sk_buff *skb;
+        struct digital_sel_req *sel_req;
+        u8 sel_cmd;
+        int rc;
+
+        skb = digital_skb_alloc(ddev, sizeof(struct digital_sel_req));
+        if (!skb)
+                return -ENOMEM;
+
+        skb_put(skb, sizeof(struct digital_sel_req));
+        sel_req = (struct digital_sel_req *)skb->data;
+
+        if (target->nfcid1_len <= 4)
+                sel_cmd = DIGITAL_CMD_SEL_REQ_CL1;
+        else if (target->nfcid1_len < 10)
+                sel_cmd = DIGITAL_CMD_SEL_REQ_CL2;
+        else
+                sel_cmd = DIGITAL_CMD_SEL_REQ_CL3;
+
+        sel_req->sel_cmd = sel_cmd;
+        sel_req->b2 = 0x70;
+        memcpy(sel_req->nfcid1, sdd_res->nfcid1, 4);
+        sel_req->bcc = sdd_res->bcc;
+
+        if (DIGITAL_DRV_CAPS_IN_CRC(ddev)) {
+                rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+                                NFC_DIGITAL_FRAMING_NFCA_STANDARD_WITH_CRC_A);
+                if (rc)
+                        goto exit;
+        } else {
+                digital_skb_add_crc_a(skb);
+        }
+
+        rc = digital_in_send_cmd(ddev, skb, 30, digital_in_recv_sel_res,
+                                 target);
+exit:
+        if (rc)
+                kfree_skb(skb);
+
+        return rc;
+}
+
+static void digital_in_recv_sdd_res(struct nfc_digital_dev *ddev, void *arg,
+                                    struct sk_buff *resp)
+{
+        struct nfc_target *target = arg;
+        struct digital_sdd_res *sdd_res;
+        int rc;
+        u8 offset, size;
+        u8 i, bcc;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        if (resp->len < DIGITAL_SDD_RES_LEN) {
+                PROTOCOL_ERR("4.7.2.8");
+                rc = -EINVAL;
+                goto exit;
+        }
+
+        sdd_res = (struct digital_sdd_res *)resp->data;
+
+        for (i = 0, bcc = 0; i < 4; i++)
+                bcc ^= sdd_res->nfcid1[i];
+
+        if (bcc != sdd_res->bcc) {
+                PROTOCOL_ERR("4.7.2.6");
+                rc = -EINVAL;
+                goto exit;
+        }
+
+        if (sdd_res->nfcid1[0] == DIGITAL_SDD_RES_CT) {
+                offset = 1;
+                size = 3;
+        } else {
+                offset = 0;
+                size = 4;
+        }
+
+        memcpy(target->nfcid1 + target->nfcid1_len, sdd_res->nfcid1 + offset,
+               size);
+        target->nfcid1_len += size;
+
+        rc = digital_in_send_sel_req(ddev, target, sdd_res);
+
+exit:
+        dev_kfree_skb(resp);
+
+        if (rc) {
+                kfree(target);
+                digital_poll_next_tech(ddev);
+        }
+}
+
+static int digital_in_send_sdd_req(struct nfc_digital_dev *ddev,
+                                   struct nfc_target *target)
+{
+        int rc;
+        struct sk_buff *skb;
+        u8 sel_cmd;
+
+        rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+                                     NFC_DIGITAL_FRAMING_NFCA_STANDARD);
+        if (rc)
+                return rc;
+
+        skb = digital_skb_alloc(ddev, 2);
+        if (!skb)
+                return -ENOMEM;
+
+        if (target->nfcid1_len == 0)
+                sel_cmd = DIGITAL_CMD_SEL_REQ_CL1;
+        else if (target->nfcid1_len == 3)
+                sel_cmd = DIGITAL_CMD_SEL_REQ_CL2;
+        else
+                sel_cmd = DIGITAL_CMD_SEL_REQ_CL3;
+
+        *skb_put(skb, sizeof(u8)) = sel_cmd;
+        *skb_put(skb, sizeof(u8)) = DIGITAL_SDD_REQ_SEL_PAR;
+
+        return digital_in_send_cmd(ddev, skb, 30, digital_in_recv_sdd_res,
+                                   target);
+}
+
+static void digital_in_recv_sens_res(struct nfc_digital_dev *ddev, void *arg,
+                                     struct sk_buff *resp)
+{
+        struct nfc_target *target = NULL;
+        int rc;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        if (resp->len < sizeof(u16)) {
+                rc = -EIO;
+                goto exit;
+        }
+
+        target = kzalloc(sizeof(struct nfc_target), GFP_KERNEL);
+        if (!target) {
+                rc = -ENOMEM;
+                goto exit;
+        }
+
+        target->sens_res = __le16_to_cpu(*(__le16 *)resp->data);
+
+        if (!DIGITAL_SENS_RES_IS_VALID(target->sens_res)) {
+                PROTOCOL_ERR("4.6.3.3");
+                rc = -EINVAL;
+                goto exit;
+        }
+
+        if (DIGITAL_SENS_RES_IS_T1T(target->sens_res))
+                rc = digital_target_found(ddev, target, NFC_PROTO_JEWEL);
+        else
+                rc = digital_in_send_sdd_req(ddev, target);
+
+exit:
+        dev_kfree_skb(resp);
+
+        if (rc) {
+                kfree(target);
+                digital_poll_next_tech(ddev);
+        }
+}
+
+int digital_in_send_sens_req(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+        struct sk_buff *skb;
+        int rc;
+
+        rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH,
+                                     NFC_DIGITAL_RF_TECH_106A);
+        if (rc)
+                return rc;
+
+        rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+                                     NFC_DIGITAL_FRAMING_NFCA_SHORT);
+        if (rc)
+                return rc;
+
+        skb = digital_skb_alloc(ddev, 1);
+        if (!skb)
+                return -ENOMEM;
+
+        *skb_put(skb, sizeof(u8)) = DIGITAL_CMD_SENS_REQ;
+
+        rc = digital_in_send_cmd(ddev, skb, 30, digital_in_recv_sens_res, NULL);
+        if (rc)
+                kfree_skb(skb);
+
+        return rc;
+}
+
+int digital_in_recv_mifare_res(struct sk_buff *resp)
+{
+        /* Successful READ command response is 16 data bytes + 2 CRC bytes long.
+         * Since the driver can't differentiate a ACK/NACK response from a valid
+         * READ response, the CRC calculation must be handled at digital level
+         * even if the driver supports it for this technology.
+         */
+        if (resp->len == DIGITAL_MIFARE_READ_RES_LEN + DIGITAL_CRC_LEN) {
+                if (digital_skb_check_crc_a(resp)) {
+                        PROTOCOL_ERR("9.4.1.2");
+                        return -EIO;
+                }
+
+                return 0;
+        }
+
+        /* ACK response (i.e. successful WRITE). */
+        if (resp->len == 1 && resp->data[0] == DIGITAL_MIFARE_ACK_RES) {
+                resp->data[0] = 0;
+                return 0;
+        }
+
+        /* NACK and any other responses are treated as error. */
+        return -EIO;
+}
+
+static void digital_in_recv_sensf_res(struct nfc_digital_dev *ddev, void *arg,
+                                   struct sk_buff *resp)
+{
+        int rc;
+        u8 proto;
+        struct nfc_target target;
+        struct digital_sensf_res *sensf_res;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        if (resp->len < DIGITAL_SENSF_RES_MIN_LENGTH) {
+                rc = -EIO;
+                goto exit;
+        }
+
+        if (!DIGITAL_DRV_CAPS_IN_CRC(ddev)) {
+                rc = digital_skb_check_crc_f(resp);
+                if (rc) {
+                        PROTOCOL_ERR("6.4.1.8");
+                        goto exit;
+                }
+        }
+
+        skb_pull(resp, 1);
+
+        memset(&target, 0, sizeof(struct nfc_target));
+
+        sensf_res = (struct digital_sensf_res *)resp->data;
+
+        memcpy(target.sensf_res, sensf_res, resp->len);
+        target.sensf_res_len = resp->len;
+
+        memcpy(target.nfcid2, sensf_res->nfcid2, NFC_NFCID2_MAXSIZE);
+        target.nfcid2_len = NFC_NFCID2_MAXSIZE;
+
+        if (target.nfcid2[0] == DIGITAL_SENSF_NFCID2_NFC_DEP_B1 &&
+            target.nfcid2[1] == DIGITAL_SENSF_NFCID2_NFC_DEP_B2)
+                proto = NFC_PROTO_NFC_DEP;
+        else
+                proto = NFC_PROTO_FELICA;
+
+        rc = digital_target_found(ddev, &target, proto);
+
+exit:
+        dev_kfree_skb(resp);
+
+        if (rc)
+                digital_poll_next_tech(ddev);
+}
+
+int digital_in_send_sensf_req(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+        struct digital_sensf_req *sensf_req;
+        struct sk_buff *skb;
+        int rc;
+        u8 size;
+
+        rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH, rf_tech);
+        if (rc)
+                return rc;
+
+        rc = digital_in_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+                                     NFC_DIGITAL_FRAMING_NFCF);
+        if (rc)
+                return rc;
+
+        size = sizeof(struct digital_sensf_req);
+
+        skb = digital_skb_alloc(ddev, size);
+        if (!skb)
+                return -ENOMEM;
+
+        skb_put(skb, size);
+
+        sensf_req = (struct digital_sensf_req *)skb->data;
+        sensf_req->cmd = DIGITAL_CMD_SENSF_REQ;
+        sensf_req->sc1 = 0xFF;
+        sensf_req->sc2 = 0xFF;
+        sensf_req->rc = 0;
+        sensf_req->tsn = 0;
+
+        *skb_push(skb, 1) = size + 1;
+
+        if (!DIGITAL_DRV_CAPS_IN_CRC(ddev))
+                digital_skb_add_crc_f(skb);
+
+        rc = digital_in_send_cmd(ddev, skb, 30, digital_in_recv_sensf_res,
+                                 NULL);
+        if (rc)
+                kfree_skb(skb);
+
+        return rc;
+}
+
+static int digital_tg_send_sel_res(struct nfc_digital_dev *ddev)
+{
+        struct sk_buff *skb;
+        int rc;
+
+        skb = digital_skb_alloc(ddev, 1);
+        if (!skb)
+                return -ENOMEM;
+
+        *skb_put(skb, 1) = DIGITAL_SEL_RES_NFC_DEP;
+
+        if (!DIGITAL_DRV_CAPS_TG_CRC(ddev))
+                digital_skb_add_crc_a(skb);
+
+        rc = digital_tg_send_cmd(ddev, skb, 300, digital_tg_recv_atr_req,
+                                 NULL);
+        if (rc)
+                kfree_skb(skb);
+
+        return rc;
+}
+
+static void digital_tg_recv_sel_req(struct nfc_digital_dev *ddev, void *arg,
+                                    struct sk_buff *resp)
+{
+        int rc;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        if (!DIGITAL_DRV_CAPS_TG_CRC(ddev)) {
+                rc = digital_skb_check_crc_a(resp);
+                if (rc) {
+                        PROTOCOL_ERR("4.4.1.3");
+                        goto exit;
+                }
+        }
+
+        /* Silently ignore SEL_REQ content and send a SEL_RES for NFC-DEP */
+
+        rc = digital_tg_send_sel_res(ddev);
+
+exit:
+        if (rc)
+                digital_poll_next_tech(ddev);
+
+        dev_kfree_skb(resp);
+}
+
+static int digital_tg_send_sdd_res(struct nfc_digital_dev *ddev)
+{
+        struct sk_buff *skb;
+        struct digital_sdd_res *sdd_res;
+        int rc, i;
+
+        skb = digital_skb_alloc(ddev, sizeof(struct digital_sdd_res));
+        if (!skb)
+                return -ENOMEM;
+
+        skb_put(skb, sizeof(struct digital_sdd_res));
+        sdd_res = (struct digital_sdd_res *)skb->data;
+
+        sdd_res->nfcid1[0] = 0x08;
+        get_random_bytes(sdd_res->nfcid1 + 1, 3);
+
+        sdd_res->bcc = 0;
+        for (i = 0; i < 4; i++)
+                sdd_res->bcc ^= sdd_res->nfcid1[i];
+
+        rc = digital_tg_send_cmd(ddev, skb, 300, digital_tg_recv_sel_req,
+                                 NULL);
+        if (rc)
+                kfree_skb(skb);
+
+        return rc;
+}
+
+static void digital_tg_recv_sdd_req(struct nfc_digital_dev *ddev, void *arg,
+                                    struct sk_buff *resp)
+{
+        u8 *sdd_req;
+        int rc;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        sdd_req = resp->data;
+
+        if (resp->len < 2 || sdd_req[0] != DIGITAL_CMD_SEL_REQ_CL1 ||
+            sdd_req[1] != DIGITAL_SDD_REQ_SEL_PAR) {
+                rc = -EINVAL;
+                goto exit;
+        }
+
+        rc = digital_tg_send_sdd_res(ddev);
+
+exit:
+        if (rc)
+                digital_poll_next_tech(ddev);
+
+        dev_kfree_skb(resp);
+}
+
+static int digital_tg_send_sens_res(struct nfc_digital_dev *ddev)
+{
+        struct sk_buff *skb;
+        u8 *sens_res;
+        int rc;
+
+        skb = digital_skb_alloc(ddev, 2);
+        if (!skb)
+                return -ENOMEM;
+
+        sens_res = skb_put(skb, 2);
+
+        sens_res[0] = (DIGITAL_SENS_RES_NFC_DEP >> 8) & 0xFF;
+        sens_res[1] = DIGITAL_SENS_RES_NFC_DEP & 0xFF;
+
+        rc = digital_tg_send_cmd(ddev, skb, 300, digital_tg_recv_sdd_req,
+                                 NULL);
+        if (rc)
+                kfree_skb(skb);
+
+        return rc;
+}
+
+void digital_tg_recv_sens_req(struct nfc_digital_dev *ddev, void *arg,
+                              struct sk_buff *resp)
+{
+        u8 sens_req;
+        int rc;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        sens_req = resp->data[0];
+
+        if (!resp->len || (sens_req != DIGITAL_CMD_SENS_REQ &&
+            sens_req != DIGITAL_CMD_ALL_REQ)) {
+                rc = -EINVAL;
+                goto exit;
+        }
+
+        rc = digital_tg_send_sens_res(ddev);
+
+exit:
+        if (rc)
+                digital_poll_next_tech(ddev);
+
+        dev_kfree_skb(resp);
+}
+
+static int digital_tg_send_sensf_res(struct nfc_digital_dev *ddev,
+                              struct digital_sensf_req *sensf_req)
+{
+        struct sk_buff *skb;
+        u8 size;
+        int rc;
+        struct digital_sensf_res *sensf_res;
+
+        size = sizeof(struct digital_sensf_res);
+
+        if (sensf_req->rc != DIGITAL_SENSF_REQ_RC_NONE)
+                size -= sizeof(sensf_res->rd);
+
+        skb = digital_skb_alloc(ddev, size);
+        if (!skb)
+                return -ENOMEM;
+
+        skb_put(skb, size);
+
+        sensf_res = (struct digital_sensf_res *)skb->data;
+
+        memset(sensf_res, 0, size);
+
+        sensf_res->cmd = DIGITAL_CMD_SENSF_RES;
+        sensf_res->nfcid2[0] = DIGITAL_SENSF_NFCID2_NFC_DEP_B1;
+        sensf_res->nfcid2[1] = DIGITAL_SENSF_NFCID2_NFC_DEP_B2;
+        get_random_bytes(&sensf_res->nfcid2[2], 6);
+
+        switch (sensf_req->rc) {
+        case DIGITAL_SENSF_REQ_RC_SC:
+                sensf_res->rd[0] = sensf_req->sc1;
+                sensf_res->rd[1] = sensf_req->sc2;
+                break;
+        case DIGITAL_SENSF_REQ_RC_AP:
+                sensf_res->rd[0] = DIGITAL_SENSF_RES_RD_AP_B1;
+                sensf_res->rd[1] = DIGITAL_SENSF_RES_RD_AP_B2;
+                break;
+        }
+
+        *skb_push(skb, sizeof(u8)) = size + 1;
+
+        if (!DIGITAL_DRV_CAPS_TG_CRC(ddev))
+                digital_skb_add_crc_f(skb);
+
+        rc = digital_tg_send_cmd(ddev, skb, 300,
+                                 digital_tg_recv_atr_req, NULL);
+        if (rc)
+                kfree_skb(skb);
+
+        return rc;
+}
+
+void digital_tg_recv_sensf_req(struct nfc_digital_dev *ddev, void *arg,
+                               struct sk_buff *resp)
+{
+        struct digital_sensf_req *sensf_req;
+        int rc;
+
+        if (IS_ERR(resp)) {
+                rc = PTR_ERR(resp);
+                resp = NULL;
+                goto exit;
+        }
+
+        if (!DIGITAL_DRV_CAPS_TG_CRC(ddev)) {
+                rc = digital_skb_check_crc_f(resp);
+                if (rc) {
+                        PROTOCOL_ERR("6.4.1.8");
+                        goto exit;
+                }
+        }
+
+        if (resp->len != sizeof(struct digital_sensf_req) + 1) {
+                rc = -EINVAL;
+                goto exit;
+        }
+
+        skb_pull(resp, 1);
+        sensf_req = (struct digital_sensf_req *)resp->data;
+
+        if (sensf_req->cmd != DIGITAL_CMD_SENSF_REQ) {
+                rc = -EINVAL;
+                goto exit;
+        }
+
+        rc = digital_tg_send_sensf_res(ddev, sensf_req);
+
+exit:
+        if (rc)
+                digital_poll_next_tech(ddev);
+
+        dev_kfree_skb(resp);
+}
+
+int digital_tg_listen_nfca(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+        int rc;
+
+        rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH, rf_tech);
+        if (rc)
+                return rc;
+
+        rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+                                     NFC_DIGITAL_FRAMING_NFCA_NFC_DEP);
+        if (rc)
+                return rc;
+
+        return digital_tg_listen(ddev, 300, digital_tg_recv_sens_req, NULL);
+}
+
+int digital_tg_listen_nfcf(struct nfc_digital_dev *ddev, u8 rf_tech)
+{
+        int rc;
+        u8 *nfcid2;
+
+        rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH, rf_tech);
+        if (rc)
+                return rc;
+
+        rc = digital_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+                                     NFC_DIGITAL_FRAMING_NFCF_NFC_DEP);
+        if (rc)
+                return rc;
+
+        nfcid2 = kzalloc(NFC_NFCID2_MAXSIZE, GFP_KERNEL);
+        if (!nfcid2)
+                return -ENOMEM;
+
+        nfcid2[0] = DIGITAL_SENSF_NFCID2_NFC_DEP_B1;
+        nfcid2[1] = DIGITAL_SENSF_NFCID2_NFC_DEP_B2;
+        get_random_bytes(nfcid2 + 2, NFC_NFCID2_MAXSIZE - 2);
+
+        return digital_tg_listen(ddev, 300, digital_tg_recv_sensf_req, nfcid2);
+}
diff --git a/net/nfc/nci/spi.c b/net/nfc/nci/spi.c
index c7cf37ba7298..f1d426f10cce 100644
--- a/net/nfc/nci/spi.c
+++ b/net/nfc/nci/spi.c
@@ -21,11 +21,8 @@
 #include <linux/export.h>
 #include <linux/spi/spi.h>
 #include <linux/crc-ccitt.h>
-#include <linux/nfc.h>
 #include <net/nfc/nci_core.h>
 
-#define NCI_SPI_HDR_LEN                 4
-#define NCI_SPI_CRC_LEN                 2
 #define NCI_SPI_ACK_SHIFT               6
 #define NCI_SPI_MSB_PAYLOAD_MASK        0x3F
 
@@ -41,54 +38,48 @@
 
 #define CRC_INIT                0xFFFF
 
-static int nci_spi_open(struct nci_dev *nci_dev)
-{
-        struct nci_spi_dev *ndev = nci_get_drvdata(nci_dev);
-
-        return ndev->ops->open(ndev);
-}
-
-static int nci_spi_close(struct nci_dev *nci_dev)
-{
-        struct nci_spi_dev *ndev = nci_get_drvdata(nci_dev);
-
-        return ndev->ops->close(ndev);
-}
-
-static int __nci_spi_send(struct nci_spi_dev *ndev, struct sk_buff *skb)
+static int __nci_spi_send(struct nci_spi *nspi, struct sk_buff *skb,
+                          int cs_change)
 {
         struct spi_message m;
         struct spi_transfer t;
 
-        t.tx_buf = skb->data;
-        t.len = skb->len;
-        t.cs_change = 0;
-        t.delay_usecs = ndev->xfer_udelay;
+        memset(&t, 0, sizeof(struct spi_transfer));
+        /* a NULL skb means we just want the SPI chip select line to raise */
+        if (skb) {
+                t.tx_buf = skb->data;
+                t.len = skb->len;
+        } else {
+                /* still set tx_buf non NULL to make the driver happy */
+                t.tx_buf = &t;
+                t.len = 0;
+        }
+        t.cs_change = cs_change;
+        t.delay_usecs = nspi->xfer_udelay;
 
         spi_message_init(&m);
         spi_message_add_tail(&t, &m);
 
-        return spi_sync(ndev->spi, &m);
+        return spi_sync(nspi->spi, &m);
 }
 
-static int nci_spi_send(struct nci_dev *nci_dev, struct sk_buff *skb)
+int nci_spi_send(struct nci_spi *nspi,
+                 struct completion *write_handshake_completion,
+                 struct sk_buff *skb)
 {
-        struct nci_spi_dev *ndev = nci_get_drvdata(nci_dev);
         unsigned int payload_len = skb->len;
         unsigned char *hdr;
         int ret;
         long completion_rc;
 
-        ndev->ops->deassert_int(ndev);
-
         /* add the NCI SPI header to the start of the buffer */
         hdr = skb_push(skb, NCI_SPI_HDR_LEN);
         hdr[0] = NCI_SPI_DIRECT_WRITE;
-        hdr[1] = ndev->acknowledge_mode;
+        hdr[1] = nspi->acknowledge_mode;
         hdr[2] = payload_len >> 8;
         hdr[3] = payload_len & 0xFF;
 
-        if (ndev->acknowledge_mode == NCI_SPI_CRC_ENABLED) {
+        if (nspi->acknowledge_mode == NCI_SPI_CRC_ENABLED) {
                 u16 crc;
 
                 crc = crc_ccitt(CRC_INIT, skb->data, skb->len);
@@ -96,123 +87,77 @@ static int nci_spi_send(struct nci_dev *nci_dev, struct sk_buff *skb)
                 *skb_put(skb, 1) = crc & 0xFF;
         }
 
-        ret = __nci_spi_send(ndev, skb);
+        if (write_handshake_completion) {
+                /* Trick SPI driver to raise chip select */
+                ret = __nci_spi_send(nspi, NULL, 1);
+                if (ret)
+                        goto done;
 
-        kfree_skb(skb);
-        ndev->ops->assert_int(ndev);
+                /* wait for NFC chip hardware handshake to complete */
+                if (wait_for_completion_timeout(write_handshake_completion,
+                                                msecs_to_jiffies(1000)) == 0) {
+                        ret = -ETIME;
+                        goto done;
+                }
+        }
 
-        if (ret != 0 || ndev->acknowledge_mode == NCI_SPI_CRC_DISABLED)
+        ret = __nci_spi_send(nspi, skb, 0);
+        if (ret != 0 || nspi->acknowledge_mode == NCI_SPI_CRC_DISABLED)
                 goto done;
 
-        init_completion(&ndev->req_completion);
-        completion_rc =
-                wait_for_completion_interruptible_timeout(&ndev->req_completion,
-                                                          NCI_SPI_SEND_TIMEOUT);
+        init_completion(&nspi->req_completion);
+        completion_rc = wait_for_completion_interruptible_timeout(
+                                                        &nspi->req_completion,
+                                                        NCI_SPI_SEND_TIMEOUT);
 
-        if (completion_rc <= 0 || ndev->req_result == ACKNOWLEDGE_NACK)
+        if (completion_rc <= 0 || nspi->req_result == ACKNOWLEDGE_NACK)
                 ret = -EIO;
 
 done:
+        kfree_skb(skb);
+
         return ret;
 }
-
-static struct nci_ops nci_spi_ops = {
-        .open = nci_spi_open,
-        .close = nci_spi_close,
-        .send = nci_spi_send,
-};
+EXPORT_SYMBOL_GPL(nci_spi_send);
 
 /* ---- Interface to NCI SPI drivers ---- */
 
 /**
- * nci_spi_allocate_device - allocate a new nci spi device
+ * nci_spi_allocate_spi - allocate a new nci spi
  *
  * @spi: SPI device
- * @ops: device operations
- * @supported_protocols: NFC protocols supported by the device
- * @supported_se: NFC Secure Elements supported by the device
- * @acknowledge_mode: Acknowledge mode used by the device
+ * @acknowledge_mode: Acknowledge mode used by the NFC device
  * @delay: delay between transactions in us
+ * @ndev: nci dev to send incoming nci frames to
  */
-struct nci_spi_dev *nci_spi_allocate_device(struct spi_device *spi,
-                                                struct nci_spi_ops *ops,
-                                                u32 supported_protocols,
-                                                u32 supported_se,
-                                                u8 acknowledge_mode,
-                                                unsigned int delay)
+struct nci_spi *nci_spi_allocate_spi(struct spi_device *spi,
+                                     u8 acknowledge_mode, unsigned int delay,
+                                     struct nci_dev *ndev)
 {
-        struct nci_spi_dev *ndev;
-        int tailroom = 0;
+        struct nci_spi *nspi;
 
-        if (!ops->open || !ops->close || !ops->assert_int || !ops->deassert_int)
+        nspi = devm_kzalloc(&spi->dev, sizeof(struct nci_spi), GFP_KERNEL);
+        if (!nspi)
                 return NULL;
 
-        if (!supported_protocols)
-                return NULL;
-
-        ndev = devm_kzalloc(&spi->dev, sizeof(struct nci_dev), GFP_KERNEL);
-        if (!ndev)
-                return NULL;
+        nspi->acknowledge_mode = acknowledge_mode;
+        nspi->xfer_udelay = delay;
 
-        ndev->ops = ops;
-        ndev->acknowledge_mode = acknowledge_mode;
-        ndev->xfer_udelay = delay;
+        nspi->spi = spi;
+        nspi->ndev = ndev;
 
-        if (acknowledge_mode == NCI_SPI_CRC_ENABLED)
-                tailroom += NCI_SPI_CRC_LEN;
-
-        ndev->nci_dev = nci_allocate_device(&nci_spi_ops, supported_protocols,
-                                            NCI_SPI_HDR_LEN, tailroom);
-        if (!ndev->nci_dev)
-                return NULL;
-
-        nci_set_drvdata(ndev->nci_dev, ndev);
-
-        return ndev;
+        return nspi;
 }
-EXPORT_SYMBOL_GPL(nci_spi_allocate_device);
+EXPORT_SYMBOL_GPL(nci_spi_allocate_spi);
 
-/**
- * nci_spi_free_device - deallocate nci spi device
- *
- * @ndev: The nci spi device to deallocate
- */
-void nci_spi_free_device(struct nci_spi_dev *ndev)
-{
-        nci_free_device(ndev->nci_dev);
-}
-EXPORT_SYMBOL_GPL(nci_spi_free_device);
-
-/**
- * nci_spi_register_device - register a nci spi device in the nfc subsystem
- *
- * @pdev: The nci spi device to register
- */
-int nci_spi_register_device(struct nci_spi_dev *ndev)
-{
-        return nci_register_device(ndev->nci_dev);
-}
-EXPORT_SYMBOL_GPL(nci_spi_register_device);
-
-/**
- * nci_spi_unregister_device - unregister a nci spi device in the nfc subsystem
- *
- * @dev: The nci spi device to unregister
- */
-void nci_spi_unregister_device(struct nci_spi_dev *ndev)
-{
-        nci_unregister_device(ndev->nci_dev);
-}
-EXPORT_SYMBOL_GPL(nci_spi_unregister_device);
-
-static int send_acknowledge(struct nci_spi_dev *ndev, u8 acknowledge)
+static int send_acknowledge(struct nci_spi *nspi, u8 acknowledge)
 {
         struct sk_buff *skb;
         unsigned char *hdr;
         u16 crc;
         int ret;
 
-        skb = nci_skb_alloc(ndev->nci_dev, 0, GFP_KERNEL);
+        skb = nci_skb_alloc(nspi->ndev, 0, GFP_KERNEL);
 
         /* add the NCI SPI header to the start of the buffer */
         hdr = skb_push(skb, NCI_SPI_HDR_LEN);
@@ -225,14 +170,14 @@ static int send_acknowledge(struct nci_spi_dev *ndev, u8 acknowledge)
         *skb_put(skb, 1) = crc >> 8;
         *skb_put(skb, 1) = crc & 0xFF;
 
-        ret = __nci_spi_send(ndev, skb);
+        ret = __nci_spi_send(nspi, skb, 0);
 
         kfree_skb(skb);
 
         return ret;
 }
 
-static struct sk_buff *__nci_spi_recv_frame(struct nci_spi_dev *ndev)
+static struct sk_buff *__nci_spi_read(struct nci_spi *nspi)
 {
         struct sk_buff *skb;
         struct spi_message m;
@@ -242,43 +187,49 @@ static struct sk_buff *__nci_spi_recv_frame(struct nci_spi_dev *ndev)
         int ret;
 
         spi_message_init(&m);
+
+        memset(&tx, 0, sizeof(struct spi_transfer));
         req[0] = NCI_SPI_DIRECT_READ;
-        req[1] = ndev->acknowledge_mode;
+        req[1] = nspi->acknowledge_mode;
         tx.tx_buf = req;
         tx.len = 2;
         tx.cs_change = 0;
         spi_message_add_tail(&tx, &m);
+
+        memset(&rx, 0, sizeof(struct spi_transfer));
         rx.rx_buf = resp_hdr;
         rx.len = 2;
         rx.cs_change = 1;
         spi_message_add_tail(&rx, &m);
-        ret = spi_sync(ndev->spi, &m);
 
+        ret = spi_sync(nspi->spi, &m);
         if (ret)
                 return NULL;
 
-        if (ndev->acknowledge_mode == NCI_SPI_CRC_ENABLED)
+        if (nspi->acknowledge_mode == NCI_SPI_CRC_ENABLED)
                 rx_len = ((resp_hdr[0] & NCI_SPI_MSB_PAYLOAD_MASK) << 8) +
                                 resp_hdr[1] + NCI_SPI_CRC_LEN;
         else
                 rx_len = (resp_hdr[0] << 8) | resp_hdr[1];
 
-        skb = nci_skb_alloc(ndev->nci_dev, rx_len, GFP_KERNEL);
+        skb = nci_skb_alloc(nspi->ndev, rx_len, GFP_KERNEL);
         if (!skb)
                 return NULL;
 
         spi_message_init(&m);
+
+        memset(&rx, 0, sizeof(struct spi_transfer));
         rx.rx_buf = skb_put(skb, rx_len);
         rx.len = rx_len;
         rx.cs_change = 0;
-        rx.delay_usecs = ndev->xfer_udelay;
+        rx.delay_usecs = nspi->xfer_udelay;
         spi_message_add_tail(&rx, &m);
-        ret = spi_sync(ndev->spi, &m);
 
+        ret = spi_sync(nspi->spi, &m);
         if (ret)
                 goto receive_error;
 
-        if (ndev->acknowledge_mode == NCI_SPI_CRC_ENABLED) {
+        if (nspi->acknowledge_mode == NCI_SPI_CRC_ENABLED) {
                 *skb_push(skb, 1) = resp_hdr[1];
                 *skb_push(skb, 1) = resp_hdr[0];
         }
@@ -318,61 +269,53 @@ static u8 nci_spi_get_ack(struct sk_buff *skb)
 }
 
 /**
- * nci_spi_recv_frame - receive frame from NCI SPI drivers
+ * nci_spi_read - read frame from NCI SPI drivers
  *
- * @ndev: The nci spi device
+ * @nspi: The nci spi
  * Context: can sleep
  *
  * This call may only be used from a context that may sleep.  The sleep
  * is non-interruptible, and has no timeout.
  *
- * It returns zero on success, else a negative error code.
+ * It returns an allocated skb containing the frame on success, or NULL.
  */
-int nci_spi_recv_frame(struct nci_spi_dev *ndev)
+struct sk_buff *nci_spi_read(struct nci_spi *nspi)
 {
         struct sk_buff *skb;
-        int ret = 0;
-
-        ndev->ops->deassert_int(ndev);
 
         /* Retrieve frame from SPI */
-        skb = __nci_spi_recv_frame(ndev);
-        if (!skb) {
-                ret = -EIO;
+        skb = __nci_spi_read(nspi);
+        if (!skb)
                 goto done;
-        }
 
-        if (ndev->acknowledge_mode == NCI_SPI_CRC_ENABLED) {
+        if (nspi->acknowledge_mode == NCI_SPI_CRC_ENABLED) {
                 if (!nci_spi_check_crc(skb)) {
-                        send_acknowledge(ndev, ACKNOWLEDGE_NACK);
+                        send_acknowledge(nspi, ACKNOWLEDGE_NACK);
                         goto done;
                 }
 
                 /* In case of acknowledged mode: if ACK or NACK received,
                  * unblock completion of latest frame sent.
                  */
-                ndev->req_result = nci_spi_get_ack(skb);
-                if (ndev->req_result)
-                        complete(&ndev->req_completion);
+                nspi->req_result = nci_spi_get_ack(skb);
+                if (nspi->req_result)
+                        complete(&nspi->req_completion);
         }
 
         /* If there is no payload (ACK/NACK only frame),
          * free the socket buffer
          */
-        if (skb->len == 0) {
+        if (!skb->len) {
                 kfree_skb(skb);
+                skb = NULL;
                 goto done;
         }
 
-        if (ndev->acknowledge_mode == NCI_SPI_CRC_ENABLED)
-                send_acknowledge(ndev, ACKNOWLEDGE_ACK);
-
-        /* Forward skb to NCI core layer */
-        ret = nci_recv_frame(ndev->nci_dev, skb);
+        if (nspi->acknowledge_mode == NCI_SPI_CRC_ENABLED)
+                send_acknowledge(nspi, ACKNOWLEDGE_ACK);
 
 done:
-        ndev->ops->assert_int(ndev);
 
-        return ret;
+        return skb;
 }
-EXPORT_SYMBOL_GPL(nci_spi_recv_frame);
+EXPORT_SYMBOL_GPL(nci_spi_read);
diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c
index 68063b2025da..84b7e3ea7b7a 100644
--- a/net/nfc/netlink.c
+++ b/net/nfc/netlink.c
@@ -58,6 +58,7 @@ static const struct nla_policy nfc_genl_policy[NFC_ATTR_MAX + 1] = {
         [NFC_ATTR_LLC_SDP] = { .type = NLA_NESTED },
         [NFC_ATTR_FIRMWARE_NAME] = { .type = NLA_STRING,
                                      .len = NFC_FIRMWARE_NAME_MAXSIZE },
+        [NFC_ATTR_SE_APDU] = { .type = NLA_BINARY },
 };
 
 static const struct nla_policy nfc_sdp_genl_policy[NFC_SDP_ATTR_MAX + 1] = {
@@ -1278,6 +1279,91 @@ static int nfc_genl_dump_ses_done(struct netlink_callback *cb)
         return 0;
 }
 
+struct se_io_ctx {
+        u32 dev_idx;
+        u32 se_idx;
+};
+
+static void se_io_cb(void *context, u8 *apdu, size_t apdu_len, int err)
+{
+        struct se_io_ctx *ctx = context;
+        struct sk_buff *msg;
+        void *hdr;
+
+        msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
+        if (!msg) {
+                kfree(ctx);
+                return;
+        }
+
+        hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0,
+                          NFC_CMD_SE_IO);
+        if (!hdr)
+                goto free_msg;
+
+        if (nla_put_u32(msg, NFC_ATTR_DEVICE_INDEX, ctx->dev_idx) ||
+            nla_put_u32(msg, NFC_ATTR_SE_INDEX, ctx->se_idx) ||
+            nla_put(msg, NFC_ATTR_SE_APDU, apdu_len, apdu))
+                goto nla_put_failure;
+
+        genlmsg_end(msg, hdr);
+
+        genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_KERNEL);
+
+        kfree(ctx);
+
+        return;
+
+nla_put_failure:
+        genlmsg_cancel(msg, hdr);
+free_msg:
+        nlmsg_free(msg);
+        kfree(ctx);
+
+        return;
+}
+
+static int nfc_genl_se_io(struct sk_buff *skb, struct genl_info *info)
+{
+        struct nfc_dev *dev;
+        struct se_io_ctx *ctx;
+        u32 dev_idx, se_idx;
+        u8 *apdu;
+        size_t apdu_len;
+
+        if (!info->attrs[NFC_ATTR_DEVICE_INDEX] ||
+            !info->attrs[NFC_ATTR_SE_INDEX] ||
+            !info->attrs[NFC_ATTR_SE_APDU])
+                return -EINVAL;
+
+        dev_idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]);
+        se_idx = nla_get_u32(info->attrs[NFC_ATTR_SE_INDEX]);
+
+        dev = nfc_get_device(dev_idx);
+        if (!dev)
+                return -ENODEV;
+
+        if (!dev->ops || !dev->ops->se_io)
+                return -ENOTSUPP;
+
+        apdu_len = nla_len(info->attrs[NFC_ATTR_SE_APDU]);
+        if (apdu_len == 0)
+                return -EINVAL;
+
+        apdu = nla_data(info->attrs[NFC_ATTR_SE_APDU]);
+        if (!apdu)
+                return -EINVAL;
+
+        ctx = kzalloc(sizeof(struct se_io_ctx), GFP_KERNEL);
+        if (!ctx)
+                return -ENOMEM;
+
+        ctx->dev_idx = dev_idx;
+        ctx->se_idx = se_idx;
+
+        return dev->ops->se_io(dev, se_idx, apdu, apdu_len, se_io_cb, ctx);
+}
+
 static struct genl_ops nfc_genl_ops[] = {
         {
                 .cmd = NFC_CMD_GET_DEVICE,
@@ -1358,6 +1444,11 @@ static struct genl_ops nfc_genl_ops[] = {
                 .done = nfc_genl_dump_ses_done,
                 .policy = nfc_genl_policy,
         },
+        {
+                .cmd = NFC_CMD_SE_IO,
+                .doit = nfc_genl_se_io,
+                .policy = nfc_genl_policy,
+        },
 };
 
 
diff --git a/net/nfc/rawsock.c b/net/nfc/rawsock.c
index 313bf1bc848a..cd958b381f96 100644
--- a/net/nfc/rawsock.c
+++ b/net/nfc/rawsock.c
@@ -142,11 +142,11 @@ static void rawsock_data_exchange_complete(void *context, struct sk_buff *skb,
 
         err = rawsock_add_header(skb);
         if (err)
-                goto error;
+                goto error_skb;
 
         err = sock_queue_rcv_skb(sk, skb);
         if (err)
-                goto error;
+                goto error_skb;
 
         spin_lock_bh(&sk->sk_write_queue.lock);
         if (!skb_queue_empty(&sk->sk_write_queue))
@@ -158,6 +158,9 @@ static void rawsock_data_exchange_complete(void *context, struct sk_buff *skb,
         sock_put(sk);
         return;
 
+error_skb:
+        kfree_skb(skb);
+
 error:
         rawsock_report_error(sk, err);
         sock_put(sk);
diff --git a/net/wireless/reg.c b/net/wireless/reg.c
index d62cb1e91475..a0ec143ba3dc 100644
--- a/net/wireless/reg.c
+++ b/net/wireless/reg.c
@@ -768,23 +768,25 @@ const struct ieee80211_reg_rule *freq_reg_info(struct wiphy *wiphy,
 }
 EXPORT_SYMBOL(freq_reg_info);
 
-#ifdef CONFIG_CFG80211_REG_DEBUG
-static const char *reg_initiator_name(enum nl80211_reg_initiator initiator)
+const char *reg_initiator_name(enum nl80211_reg_initiator initiator)
 {
         switch (initiator) {
         case NL80211_REGDOM_SET_BY_CORE:
-                return "Set by core";
+                return "core";
         case NL80211_REGDOM_SET_BY_USER:
-                return "Set by user";
+                return "user";
         case NL80211_REGDOM_SET_BY_DRIVER:
-                return "Set by driver";
+                return "driver";
         case NL80211_REGDOM_SET_BY_COUNTRY_IE:
-                return "Set by country IE";
+                return "country IE";
         default:
                 WARN_ON(1);
-                return "Set by bug";
+                return "bug";
         }
 }
+EXPORT_SYMBOL(reg_initiator_name);
+
+#ifdef CONFIG_CFG80211_REG_DEBUG
 
 static void chan_reg_rule_print_dbg(struct ieee80211_channel *chan,
                                     const struct ieee80211_reg_rule *reg_rule)
@@ -979,14 +981,17 @@ static bool ignore_reg_update(struct wiphy *wiphy,
         struct regulatory_request *lr = get_last_request();
 
         if (!lr) {
-                REG_DBG_PRINT("Ignoring regulatory request %s since last_request is not set\n",
+                REG_DBG_PRINT("Ignoring regulatory request set by %s "
+                              "since last_request is not set\n",
                               reg_initiator_name(initiator));
                 return true;
         }
 
         if (initiator == NL80211_REGDOM_SET_BY_CORE &&
             wiphy->flags & WIPHY_FLAG_CUSTOM_REGULATORY) {
-                REG_DBG_PRINT("Ignoring regulatory request %s since the driver uses its own custom regulatory domain\n",
+                REG_DBG_PRINT("Ignoring regulatory request set by %s "
+                              "since the driver uses its own custom "
+                              "regulatory domain\n",
                               reg_initiator_name(initiator));
                 return true;
         }
@@ -998,7 +1003,9 @@ static bool ignore_reg_update(struct wiphy *wiphy,
         if (wiphy->flags & WIPHY_FLAG_STRICT_REGULATORY && !wiphy->regd &&
             initiator != NL80211_REGDOM_SET_BY_COUNTRY_IE &&
             !is_world_regdom(lr->alpha2)) {
-                REG_DBG_PRINT("Ignoring regulatory request %s since the driver requires its own regulatory domain to be set first\n",
+                REG_DBG_PRINT("Ignoring regulatory request set by %s "
+                              "since the driver requires its own regulatory "
+                              "domain to be set first\n",
                               reg_initiator_name(initiator));
                 return true;
         }