24 files changed, 4186 insertions, 189 deletions

diff --git a/Documentation/00-INDEX b/Documentation/00-INDEX
index d05737aaa84b..06b982affe76 100644
--- a/Documentation/00-INDEX
+++ b/Documentation/00-INDEX
@@ -82,6 +82,8 @@ block/
         - info on the Block I/O (BIO) layer.
 blockdev/
         - info on block devices & drivers
+btmrvl.txt
+        - info on Marvell Bluetooth driver usage.
 cachetlb.txt
         - describes the cache/TLB flushing interfaces Linux uses.
 cdrom/
diff --git a/Documentation/btmrvl.txt b/Documentation/btmrvl.txt
new file mode 100644
index 000000000000..34916a46c099
--- /dev/null
+++ b/Documentation/btmrvl.txt
@@ -0,0 +1,119 @@
+=======================================================================
+                README for btmrvl driver
+=======================================================================
+
+
+All commands are used via debugfs interface.
+
+=====================
+Set/get driver configurations:
+
+Path:   /debug/btmrvl/config/
+
+gpiogap=[n]
+hscfgcmd
+        These commands are used to configure the host sleep parameters.
+        bit 8:0  -- Gap
+        bit 16:8 -- GPIO
+
+        where GPIO is the pin number of GPIO used to wake up the host.
+        It could be any valid GPIO pin# (e.g. 0-7) or 0xff (SDIO interface
+        wakeup will be used instead).
+
+        where Gap is the gap in milli seconds between wakeup signal and
+        wakeup event, or 0xff for special host sleep setting.
+
+        Usage:
+                # Use SDIO interface to wake up the host and set GAP to 0x80:
+                echo 0xff80 > /debug/btmrvl/config/gpiogap
+                echo 1 > /debug/btmrvl/config/hscfgcmd
+
+                # Use GPIO pin #3 to wake up the host and set GAP to 0xff:
+                echo 0x03ff >  /debug/btmrvl/config/gpiogap
+                echo 1 > /debug/btmrvl/config/hscfgcmd
+
+psmode=[n]
+pscmd
+        These commands are used to enable/disable auto sleep mode
+
+        where the option is:
+                        1       -- Enable auto sleep mode
+                        0       -- Disable auto sleep mode
+
+        Usage:
+                # Enable auto sleep mode
+                echo 1 > /debug/btmrvl/config/psmode
+                echo 1 > /debug/btmrvl/config/pscmd
+
+                # Disable auto sleep mode
+                echo 0 > /debug/btmrvl/config/psmode
+                echo 1 > /debug/btmrvl/config/pscmd
+
+
+hsmode=[n]
+hscmd
+        These commands are used to enable host sleep or wake up firmware
+
+        where the option is:
+                        1       -- Enable host sleep
+                        0       -- Wake up firmware
+
+        Usage:
+                # Enable host sleep
+                echo 1 > /debug/btmrvl/config/hsmode
+                echo 1 > /debug/btmrvl/config/hscmd
+
+                # Wake up firmware
+                echo 0 > /debug/btmrvl/config/hsmode
+                echo 1 > /debug/btmrvl/config/hscmd
+
+
+======================
+Get driver status:
+
+Path:   /debug/btmrvl/status/
+
+Usage:
+        cat /debug/btmrvl/status/<args>
+
+where the args are:
+
+curpsmode
+        This command displays current auto sleep status.
+
+psstate
+        This command display the power save state.
+
+hsstate
+        This command display the host sleep state.
+
+txdnldrdy
+        This command displays the value of Tx download ready flag.
+
+
+=====================
+
+Use hcitool to issue raw hci command, refer to hcitool manual
+
+        Usage: Hcitool cmd <ogf> <ocf> [Parameters]
+
+        Interface Control Command
+        hcitool cmd 0x3f 0x5b 0xf5 0x01 0x00    --Enable All interface
+        hcitool cmd 0x3f 0x5b 0xf5 0x01 0x01    --Enable Wlan interface
+        hcitool cmd 0x3f 0x5b 0xf5 0x01 0x02    --Enable BT interface
+        hcitool cmd 0x3f 0x5b 0xf5 0x00 0x00    --Disable All interface
+        hcitool cmd 0x3f 0x5b 0xf5 0x00 0x01    --Disable Wlan interface
+        hcitool cmd 0x3f 0x5b 0xf5 0x00 0x02    --Disable BT interface
+
+=======================================================================
+
+
+SD8688 firmware:
+
+/lib/firmware/sd8688_helper.bin
+/lib/firmware/sd8688.bin
+
+
+The images can be downloaded from:
+
+git.infradead.org/users/dwmw2/linux-firmware.git/libertas/
diff --git a/drivers/bluetooth/Kconfig b/drivers/bluetooth/Kconfig
index 1164837bb781..652367aa6546 100644
--- a/drivers/bluetooth/Kconfig
+++ b/drivers/bluetooth/Kconfig
@@ -170,5 +170,30 @@ config BT_HCIVHCI
           Say Y here to compile support for virtual HCI devices into the
           kernel or say M to compile it as module (hci_vhci).
 
+config BT_MRVL
+        tristate "Marvell Bluetooth driver support"
+        help
+          The core driver to support Marvell Bluetooth devices.
+
+          This driver is required if you want to support
+          Marvell Bluetooth devices, such as 8688.
+
+          Say Y here to compile Marvell Bluetooth driver
+          into the kernel or say M to compile it as module.
+
+config BT_MRVL_SDIO
+        tristate "Marvell BT-over-SDIO driver"
+        depends on BT_MRVL && MMC
+        select FW_LOADER
+        help
+          The driver for Marvell Bluetooth chipsets with SDIO interface.
+
+          This driver is required if you want to use Marvell Bluetooth
+          devices with SDIO interface. Currently only SD8688 chipset is
+          supported.
+
+          Say Y here to compile support for Marvell BT-over-SDIO driver
+          into the kernel or say M to compile it as module.
+
 endmenu
 
diff --git a/drivers/bluetooth/Makefile b/drivers/bluetooth/Makefile
index 16930f93d1ca..b3f57d2d4eb0 100644
--- a/drivers/bluetooth/Makefile
+++ b/drivers/bluetooth/Makefile
@@ -15,6 +15,12 @@ obj-$(CONFIG_BT_HCIBTUART)	+= btuart_cs.o
 obj-$(CONFIG_BT_HCIBTUSB)       += btusb.o
 obj-$(CONFIG_BT_HCIBTSDIO)      += btsdio.o
 
+obj-$(CONFIG_BT_MRVL)           += btmrvl.o
+obj-$(CONFIG_BT_MRVL_SDIO)      += btmrvl_sdio.o
+
+btmrvl-y                        := btmrvl_main.o
+btmrvl-$(CONFIG_DEBUG_FS)       += btmrvl_debugfs.o
+
 hci_uart-y                              := hci_ldisc.o
 hci_uart-$(CONFIG_BT_HCIUART_H4)        += hci_h4.o
 hci_uart-$(CONFIG_BT_HCIUART_BCSP)      += hci_bcsp.o
diff --git a/drivers/bluetooth/btmrvl_debugfs.c b/drivers/bluetooth/btmrvl_debugfs.c
new file mode 100644
index 000000000000..4617bd12f63b
--- /dev/null
+++ b/drivers/bluetooth/btmrvl_debugfs.c
@@ -0,0 +1,432 @@
+/**
+ * Marvell Bluetooth driver: debugfs related functions
+ *
+ * Copyright (C) 2009, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ **/
+
+#include <linux/debugfs.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "btmrvl_drv.h"
+
+struct btmrvl_debugfs_data {
+        struct dentry *root_dir, *config_dir, *status_dir;
+
+        /* config */
+        struct dentry *drvdbg;
+        struct dentry *psmode;
+        struct dentry *pscmd;
+        struct dentry *hsmode;
+        struct dentry *hscmd;
+        struct dentry *gpiogap;
+        struct dentry *hscfgcmd;
+
+        /* status */
+        struct dentry *curpsmode;
+        struct dentry *hsstate;
+        struct dentry *psstate;
+        struct dentry *txdnldready;
+};
+
+static int btmrvl_open_generic(struct inode *inode, struct file *file)
+{
+        file->private_data = inode->i_private;
+        return 0;
+}
+
+static ssize_t btmrvl_hscfgcmd_write(struct file *file,
+                        const char __user *ubuf, size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        long result, ret;
+
+        memset(buf, 0, sizeof(buf));
+
+        if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
+                return -EFAULT;
+
+        ret = strict_strtol(buf, 10, &result);
+
+        priv->btmrvl_dev.hscfgcmd = result;
+
+        if (priv->btmrvl_dev.hscfgcmd) {
+                btmrvl_prepare_command(priv);
+                wake_up_interruptible(&priv->main_thread.wait_q);
+        }
+
+        return count;
+}
+
+static ssize_t btmrvl_hscfgcmd_read(struct file *file, char __user *userbuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        int ret;
+
+        ret = snprintf(buf, sizeof(buf) - 1, "%d\n",
+                                                priv->btmrvl_dev.hscfgcmd);
+
+        return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
+}
+
+static const struct file_operations btmrvl_hscfgcmd_fops = {
+        .read   = btmrvl_hscfgcmd_read,
+        .write  = btmrvl_hscfgcmd_write,
+        .open   = btmrvl_open_generic,
+};
+
+static ssize_t btmrvl_psmode_write(struct file *file, const char __user *ubuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        long result, ret;
+
+        memset(buf, 0, sizeof(buf));
+
+        if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
+                return -EFAULT;
+
+        ret = strict_strtol(buf, 10, &result);
+
+        priv->btmrvl_dev.psmode = result;
+
+        return count;
+}
+
+static ssize_t btmrvl_psmode_read(struct file *file, char __user *userbuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        int ret;
+
+        ret = snprintf(buf, sizeof(buf) - 1, "%d\n",
+                                                priv->btmrvl_dev.psmode);
+
+        return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
+}
+
+static const struct file_operations btmrvl_psmode_fops = {
+        .read   = btmrvl_psmode_read,
+        .write  = btmrvl_psmode_write,
+        .open   = btmrvl_open_generic,
+};
+
+static ssize_t btmrvl_pscmd_write(struct file *file, const char __user *ubuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        long result, ret;
+
+        memset(buf, 0, sizeof(buf));
+
+        if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
+                return -EFAULT;
+
+        ret = strict_strtol(buf, 10, &result);
+
+        priv->btmrvl_dev.pscmd = result;
+
+        if (priv->btmrvl_dev.pscmd) {
+                btmrvl_prepare_command(priv);
+                wake_up_interruptible(&priv->main_thread.wait_q);
+        }
+
+        return count;
+
+}
+
+static ssize_t btmrvl_pscmd_read(struct file *file, char __user *userbuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        int ret;
+
+        ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->btmrvl_dev.pscmd);
+
+        return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
+}
+
+static const struct file_operations btmrvl_pscmd_fops = {
+        .read = btmrvl_pscmd_read,
+        .write = btmrvl_pscmd_write,
+        .open = btmrvl_open_generic,
+};
+
+static ssize_t btmrvl_gpiogap_write(struct file *file, const char __user *ubuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        long result, ret;
+
+        memset(buf, 0, sizeof(buf));
+
+        if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
+                return -EFAULT;
+
+        ret = strict_strtol(buf, 16, &result);
+
+        priv->btmrvl_dev.gpio_gap = result;
+
+        return count;
+}
+
+static ssize_t btmrvl_gpiogap_read(struct file *file, char __user *userbuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        int ret;
+
+        ret = snprintf(buf, sizeof(buf) - 1, "0x%x\n",
+                                                priv->btmrvl_dev.gpio_gap);
+
+        return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
+}
+
+static const struct file_operations btmrvl_gpiogap_fops = {
+        .read   = btmrvl_gpiogap_read,
+        .write  = btmrvl_gpiogap_write,
+        .open   = btmrvl_open_generic,
+};
+
+static ssize_t btmrvl_hscmd_write(struct file *file, const char __user *ubuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = (struct btmrvl_private *) file->private_data;
+        char buf[16];
+        long result, ret;
+
+        memset(buf, 0, sizeof(buf));
+
+        if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
+                return -EFAULT;
+
+        ret = strict_strtol(buf, 10, &result);
+
+        priv->btmrvl_dev.hscmd = result;
+        if (priv->btmrvl_dev.hscmd) {
+                btmrvl_prepare_command(priv);
+                wake_up_interruptible(&priv->main_thread.wait_q);
+        }
+
+        return count;
+}
+
+static ssize_t btmrvl_hscmd_read(struct file *file, char __user *userbuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        int ret;
+
+        ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->btmrvl_dev.hscmd);
+
+        return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
+}
+
+static const struct file_operations btmrvl_hscmd_fops = {
+        .read   = btmrvl_hscmd_read,
+        .write  = btmrvl_hscmd_write,
+        .open   = btmrvl_open_generic,
+};
+
+static ssize_t btmrvl_hsmode_write(struct file *file, const char __user *ubuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        long result, ret;
+
+        memset(buf, 0, sizeof(buf));
+
+        if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
+                return -EFAULT;
+
+        ret = strict_strtol(buf, 10, &result);
+
+        priv->btmrvl_dev.hsmode = result;
+
+        return count;
+}
+
+static ssize_t btmrvl_hsmode_read(struct file *file, char __user * userbuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        int ret;
+
+        ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->btmrvl_dev.hsmode);
+
+        return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
+}
+
+static const struct file_operations btmrvl_hsmode_fops = {
+        .read   = btmrvl_hsmode_read,
+        .write  = btmrvl_hsmode_write,
+        .open   = btmrvl_open_generic,
+};
+
+static ssize_t btmrvl_curpsmode_read(struct file *file, char __user *userbuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        int ret;
+
+        ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->adapter->psmode);
+
+        return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
+}
+
+static const struct file_operations btmrvl_curpsmode_fops = {
+        .read   = btmrvl_curpsmode_read,
+        .open   = btmrvl_open_generic,
+};
+
+static ssize_t btmrvl_psstate_read(struct file *file, char __user * userbuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        int ret;
+
+        ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->adapter->ps_state);
+
+        return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
+}
+
+static const struct file_operations btmrvl_psstate_fops = {
+        .read   = btmrvl_psstate_read,
+        .open   = btmrvl_open_generic,
+};
+
+static ssize_t btmrvl_hsstate_read(struct file *file, char __user *userbuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        int ret;
+
+        ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->adapter->hs_state);
+
+        return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
+}
+
+static const struct file_operations btmrvl_hsstate_fops = {
+        .read   = btmrvl_hsstate_read,
+        .open   = btmrvl_open_generic,
+};
+
+static ssize_t btmrvl_txdnldready_read(struct file *file, char __user *userbuf,
+                                                size_t count, loff_t *ppos)
+{
+        struct btmrvl_private *priv = file->private_data;
+        char buf[16];
+        int ret;
+
+        ret = snprintf(buf, sizeof(buf) - 1, "%d\n",
+                                        priv->btmrvl_dev.tx_dnld_rdy);
+
+        return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
+}
+
+static const struct file_operations btmrvl_txdnldready_fops = {
+        .read   = btmrvl_txdnldready_read,
+        .open   = btmrvl_open_generic,
+};
+
+void btmrvl_debugfs_init(struct hci_dev *hdev)
+{
+        struct btmrvl_private *priv = hdev->driver_data;
+        struct btmrvl_debugfs_data *dbg;
+
+        dbg = kzalloc(sizeof(*dbg), GFP_KERNEL);
+        priv->debugfs_data = dbg;
+
+        if (!dbg) {
+                BT_ERR("Can not allocate memory for btmrvl_debugfs_data.");
+                return;
+        }
+
+        dbg->root_dir = debugfs_create_dir("btmrvl", NULL);
+
+        dbg->config_dir = debugfs_create_dir("config", dbg->root_dir);
+
+        dbg->psmode = debugfs_create_file("psmode", 0644, dbg->config_dir,
+                                hdev->driver_data, &btmrvl_psmode_fops);
+        dbg->pscmd = debugfs_create_file("pscmd", 0644, dbg->config_dir,
+                                hdev->driver_data, &btmrvl_pscmd_fops);
+        dbg->gpiogap = debugfs_create_file("gpiogap", 0644, dbg->config_dir,
+                                hdev->driver_data, &btmrvl_gpiogap_fops);
+        dbg->hsmode =  debugfs_create_file("hsmode", 0644, dbg->config_dir,
+                                hdev->driver_data, &btmrvl_hsmode_fops);
+        dbg->hscmd = debugfs_create_file("hscmd", 0644, dbg->config_dir,
+                                hdev->driver_data, &btmrvl_hscmd_fops);
+        dbg->hscfgcmd = debugfs_create_file("hscfgcmd", 0644, dbg->config_dir,
+                                hdev->driver_data, &btmrvl_hscfgcmd_fops);
+
+        dbg->status_dir = debugfs_create_dir("status", dbg->root_dir);
+        dbg->curpsmode = debugfs_create_file("curpsmode", 0444,
+                                                dbg->status_dir,
+                                                hdev->driver_data,
+                                                &btmrvl_curpsmode_fops);
+        dbg->psstate = debugfs_create_file("psstate", 0444, dbg->status_dir,
+                                hdev->driver_data, &btmrvl_psstate_fops);
+        dbg->hsstate = debugfs_create_file("hsstate", 0444, dbg->status_dir,
+                                hdev->driver_data, &btmrvl_hsstate_fops);
+        dbg->txdnldready = debugfs_create_file("txdnldready", 0444,
+                                                dbg->status_dir,
+                                                hdev->driver_data,
+                                                &btmrvl_txdnldready_fops);
+}
+
+void btmrvl_debugfs_remove(struct hci_dev *hdev)
+{
+        struct btmrvl_private *priv = hdev->driver_data;
+        struct btmrvl_debugfs_data *dbg = priv->debugfs_data;
+
+        if (!dbg)
+                return;
+
+        debugfs_remove(dbg->psmode);
+        debugfs_remove(dbg->pscmd);
+        debugfs_remove(dbg->gpiogap);
+        debugfs_remove(dbg->hsmode);
+        debugfs_remove(dbg->hscmd);
+        debugfs_remove(dbg->hscfgcmd);
+        debugfs_remove(dbg->config_dir);
+
+        debugfs_remove(dbg->curpsmode);
+        debugfs_remove(dbg->psstate);
+        debugfs_remove(dbg->hsstate);
+        debugfs_remove(dbg->txdnldready);
+        debugfs_remove(dbg->status_dir);
+
+        debugfs_remove(dbg->root_dir);
+
+        kfree(dbg);
+}
diff --git a/drivers/bluetooth/btmrvl_drv.h b/drivers/bluetooth/btmrvl_drv.h
new file mode 100644
index 000000000000..411c7a77082d
--- /dev/null
+++ b/drivers/bluetooth/btmrvl_drv.h
@@ -0,0 +1,139 @@
+/*
+ * Marvell Bluetooth driver: global definitions & declarations
+ *
+ * Copyright (C) 2009, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ *
+ */
+
+#include <linux/kthread.h>
+#include <linux/bitops.h>
+#include <net/bluetooth/bluetooth.h>
+
+#define BTM_HEADER_LEN                  4
+#define BTM_UPLD_SIZE                   2312
+
+/* Time to wait until Host Sleep state change in millisecond */
+#define WAIT_UNTIL_HS_STATE_CHANGED     5000
+/* Time to wait for command response in millisecond */
+#define WAIT_UNTIL_CMD_RESP             5000
+
+struct btmrvl_thread {
+        struct task_struct *task;
+        wait_queue_head_t wait_q;
+        void *priv;
+};
+
+struct btmrvl_device {
+        void *card;
+        struct hci_dev *hcidev;
+
+        u8 tx_dnld_rdy;
+
+        u8 psmode;
+        u8 pscmd;
+        u8 hsmode;
+        u8 hscmd;
+
+        /* Low byte is gap, high byte is GPIO */
+        u16 gpio_gap;
+
+        u8 hscfgcmd;
+        u8 sendcmdflag;
+};
+
+struct btmrvl_adapter {
+        u32 int_count;
+        struct sk_buff_head tx_queue;
+        u8 psmode;
+        u8 ps_state;
+        u8 hs_state;
+        u8 wakeup_tries;
+        wait_queue_head_t cmd_wait_q;
+        u8 cmd_complete;
+};
+
+struct btmrvl_private {
+        struct btmrvl_device btmrvl_dev;
+        struct btmrvl_adapter *adapter;
+        struct btmrvl_thread main_thread;
+        int (*hw_host_to_card) (struct btmrvl_private *priv,
+                                u8 *payload, u16 nb);
+        int (*hw_wakeup_firmware) (struct btmrvl_private *priv);
+        spinlock_t driver_lock;         /* spinlock used by driver */
+#ifdef CONFIG_DEBUG_FS
+        void *debugfs_data;
+#endif
+};
+
+#define MRVL_VENDOR_PKT                 0xFE
+
+/* Bluetooth commands  */
+#define BT_CMD_AUTO_SLEEP_MODE          0x23
+#define BT_CMD_HOST_SLEEP_CONFIG        0x59
+#define BT_CMD_HOST_SLEEP_ENABLE        0x5A
+#define BT_CMD_MODULE_CFG_REQ           0x5B
+
+/* Sub-commands: Module Bringup/Shutdown Request */
+#define MODULE_BRINGUP_REQ              0xF1
+#define MODULE_SHUTDOWN_REQ             0xF2
+
+#define BT_EVENT_POWER_STATE            0x20
+
+/* Bluetooth Power States */
+#define BT_PS_ENABLE                    0x02
+#define BT_PS_DISABLE                   0x03
+#define BT_PS_SLEEP                     0x01
+
+#define OGF                             0x3F
+
+/* Host Sleep states */
+#define HS_ACTIVATED                    0x01
+#define HS_DEACTIVATED                  0x00
+
+/* Power Save modes */
+#define PS_SLEEP                        0x01
+#define PS_AWAKE                        0x00
+
+struct btmrvl_cmd {
+        __le16 ocf_ogf;
+        u8 length;
+        u8 data[4];
+} __attribute__ ((packed));
+
+struct btmrvl_event {
+        u8 ec;          /* event counter */
+        u8 length;
+        u8 data[4];
+} __attribute__ ((packed));
+
+/* Prototype of global function */
+
+struct btmrvl_private *btmrvl_add_card(void *card);
+int btmrvl_remove_card(struct btmrvl_private *priv);
+
+void btmrvl_interrupt(struct btmrvl_private *priv);
+
+void btmrvl_check_evtpkt(struct btmrvl_private *priv, struct sk_buff *skb);
+int btmrvl_process_event(struct btmrvl_private *priv, struct sk_buff *skb);
+
+int btmrvl_send_module_cfg_cmd(struct btmrvl_private *priv, int subcmd);
+int btmrvl_prepare_command(struct btmrvl_private *priv);
+
+#ifdef CONFIG_DEBUG_FS
+void btmrvl_debugfs_init(struct hci_dev *hdev);
+void btmrvl_debugfs_remove(struct hci_dev *hdev);
+#endif
diff --git a/drivers/bluetooth/btmrvl_main.c b/drivers/bluetooth/btmrvl_main.c
new file mode 100644
index 000000000000..e605563b4eaa
--- /dev/null
+++ b/drivers/bluetooth/btmrvl_main.c
@@ -0,0 +1,624 @@
+/**
+ * Marvell Bluetooth driver
+ *
+ * Copyright (C) 2009, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ **/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "btmrvl_drv.h"
+
+#define VERSION "1.0"
+
+/*
+ * This function is called by interface specific interrupt handler.
+ * It updates Power Save & Host Sleep states, and wakes up the main
+ * thread.
+ */
+void btmrvl_interrupt(struct btmrvl_private *priv)
+{
+        priv->adapter->ps_state = PS_AWAKE;
+
+        priv->adapter->wakeup_tries = 0;
+
+        priv->adapter->int_count++;
+
+        wake_up_interruptible(&priv->main_thread.wait_q);
+}
+EXPORT_SYMBOL_GPL(btmrvl_interrupt);
+
+void btmrvl_check_evtpkt(struct btmrvl_private *priv, struct sk_buff *skb)
+{
+        struct hci_event_hdr *hdr = (void *) skb->data;
+        struct hci_ev_cmd_complete *ec;
+        u16 opcode, ocf;
+
+        if (hdr->evt == HCI_EV_CMD_COMPLETE) {
+                ec = (void *) (skb->data + HCI_EVENT_HDR_SIZE);
+                opcode = __le16_to_cpu(ec->opcode);
+                ocf = hci_opcode_ocf(opcode);
+                if (ocf == BT_CMD_MODULE_CFG_REQ &&
+                                        priv->btmrvl_dev.sendcmdflag) {
+                        priv->btmrvl_dev.sendcmdflag = false;
+                        priv->adapter->cmd_complete = true;
+                        wake_up_interruptible(&priv->adapter->cmd_wait_q);
+                }
+        }
+}
+EXPORT_SYMBOL_GPL(btmrvl_check_evtpkt);
+
+int btmrvl_process_event(struct btmrvl_private *priv, struct sk_buff *skb)
+{
+        struct btmrvl_adapter *adapter = priv->adapter;
+        struct btmrvl_event *event;
+        u8 ret = 0;
+
+        event = (struct btmrvl_event *) skb->data;
+        if (event->ec != 0xff) {
+                BT_DBG("Not Marvell Event=%x", event->ec);
+                ret = -EINVAL;
+                goto exit;
+        }
+
+        switch (event->data[0]) {
+        case BT_CMD_AUTO_SLEEP_MODE:
+                if (!event->data[2]) {
+                        if (event->data[1] == BT_PS_ENABLE)
+                                adapter->psmode = 1;
+                        else
+                                adapter->psmode = 0;
+                        BT_DBG("PS Mode:%s",
+                                (adapter->psmode) ? "Enable" : "Disable");
+                } else {
+                        BT_DBG("PS Mode command failed");
+                }
+                break;
+
+        case BT_CMD_HOST_SLEEP_CONFIG:
+                if (!event->data[3])
+                        BT_DBG("gpio=%x, gap=%x", event->data[1],
+                                                        event->data[2]);
+                else
+                        BT_DBG("HSCFG command failed");
+                break;
+
+        case BT_CMD_HOST_SLEEP_ENABLE:
+                if (!event->data[1]) {
+                        adapter->hs_state = HS_ACTIVATED;
+                        if (adapter->psmode)
+                                adapter->ps_state = PS_SLEEP;
+                        wake_up_interruptible(&adapter->cmd_wait_q);
+                        BT_DBG("HS ACTIVATED!");
+                } else {
+                        BT_DBG("HS Enable failed");
+                }
+                break;
+
+        case BT_CMD_MODULE_CFG_REQ:
+                if (priv->btmrvl_dev.sendcmdflag &&
+                                event->data[1] == MODULE_BRINGUP_REQ) {
+                        BT_DBG("EVENT:%s", (event->data[2]) ?
+                                "Bring-up failed" : "Bring-up succeed");
+                } else if (priv->btmrvl_dev.sendcmdflag &&
+                                event->data[1] == MODULE_SHUTDOWN_REQ) {
+                        BT_DBG("EVENT:%s", (event->data[2]) ?
+                                "Shutdown failed" : "Shutdown succeed");
+                } else {
+                        BT_DBG("BT_CMD_MODULE_CFG_REQ resp for APP");
+                        ret = -EINVAL;
+                }
+                break;
+
+        case BT_EVENT_POWER_STATE:
+                if (event->data[1] == BT_PS_SLEEP)
+                        adapter->ps_state = PS_SLEEP;
+                BT_DBG("EVENT:%s",
+                        (adapter->ps_state) ? "PS_SLEEP" : "PS_AWAKE");
+                break;
+
+        default:
+                BT_DBG("Unknown Event=%d", event->data[0]);
+                ret = -EINVAL;
+                break;
+        }
+
+exit:
+        if (!ret)
+                kfree_skb(skb);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(btmrvl_process_event);
+
+int btmrvl_send_module_cfg_cmd(struct btmrvl_private *priv, int subcmd)
+{
+        struct sk_buff *skb;
+        struct btmrvl_cmd *cmd;
+        int ret = 0;
+
+        skb = bt_skb_alloc(sizeof(*cmd), GFP_ATOMIC);
+        if (skb == NULL) {
+                BT_ERR("No free skb");
+                return -ENOMEM;
+        }
+
+        cmd = (struct btmrvl_cmd *) skb_put(skb, sizeof(*cmd));
+        cmd->ocf_ogf = cpu_to_le16(hci_opcode_pack(OGF, BT_CMD_MODULE_CFG_REQ));
+        cmd->length = 1;
+        cmd->data[0] = subcmd;
+
+        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;
+
+        priv->adapter->cmd_complete = false;
+
+        BT_DBG("Queue module cfg Command");
+
+        wake_up_interruptible(&priv->main_thread.wait_q);
+
+        if (!wait_event_interruptible_timeout(priv->adapter->cmd_wait_q,
+                                priv->adapter->cmd_complete,
+                                msecs_to_jiffies(WAIT_UNTIL_CMD_RESP))) {
+                ret = -ETIMEDOUT;
+                BT_ERR("module_cfg_cmd(%x): timeout: %d",
+                                        subcmd, priv->btmrvl_dev.sendcmdflag);
+        }
+
+        BT_DBG("module cfg Command done");
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(btmrvl_send_module_cfg_cmd);
+
+static int btmrvl_enable_hs(struct btmrvl_private *priv)
+{
+        struct sk_buff *skb;
+        struct btmrvl_cmd *cmd;
+        int ret = 0;
+
+        skb = bt_skb_alloc(sizeof(*cmd), GFP_ATOMIC);
+        if (skb == NULL) {
+                BT_ERR("No free skb");
+                return -ENOMEM;
+        }
+
+        cmd = (struct btmrvl_cmd *) skb_put(skb, sizeof(*cmd));
+        cmd->ocf_ogf = cpu_to_le16(hci_opcode_pack(OGF, BT_CMD_HOST_SLEEP_ENABLE));
+        cmd->length = 0;
+
+        bt_cb(skb)->pkt_type = MRVL_VENDOR_PKT;
+
+        skb->dev = (void *) priv->btmrvl_dev.hcidev;
+        skb_queue_head(&priv->adapter->tx_queue, skb);
+
+        BT_DBG("Queue hs enable Command");
+
+        wake_up_interruptible(&priv->main_thread.wait_q);
+
+        if (!wait_event_interruptible_timeout(priv->adapter->cmd_wait_q,
+                        priv->adapter->hs_state,
+                        msecs_to_jiffies(WAIT_UNTIL_HS_STATE_CHANGED))) {
+                ret = -ETIMEDOUT;
+                BT_ERR("timeout: %d, %d,%d", priv->adapter->hs_state,
+                                                priv->adapter->ps_state,
+                                                priv->adapter->wakeup_tries);
+        }
+
+        return ret;
+}
+
+int btmrvl_prepare_command(struct btmrvl_private *priv)
+{
+        struct sk_buff *skb = NULL;
+        struct btmrvl_cmd *cmd;
+        int ret = 0;
+
+        if (priv->btmrvl_dev.hscfgcmd) {
+                priv->btmrvl_dev.hscfgcmd = 0;
+
+                skb = bt_skb_alloc(sizeof(*cmd), GFP_ATOMIC);
+                if (skb == NULL) {
+                        BT_ERR("No free skb");
+                        return -ENOMEM;
+                }
+
+                cmd = (struct btmrvl_cmd *) skb_put(skb, sizeof(*cmd));
+                cmd->ocf_ogf = cpu_to_le16(hci_opcode_pack(OGF, BT_CMD_HOST_SLEEP_CONFIG));
+                cmd->length = 2;
+                cmd->data[0] = (priv->btmrvl_dev.gpio_gap & 0xff00) >> 8;
+                cmd->data[1] = (u8) (priv->btmrvl_dev.gpio_gap & 0x00ff);
+
+                bt_cb(skb)->pkt_type = MRVL_VENDOR_PKT;
+
+                skb->dev = (void *) priv->btmrvl_dev.hcidev;
+                skb_queue_head(&priv->adapter->tx_queue, skb);
+
+                BT_DBG("Queue HSCFG Command, gpio=0x%x, gap=0x%x",
+                                                cmd->data[0], cmd->data[1]);
+        }
+
+        if (priv->btmrvl_dev.pscmd) {
+                priv->btmrvl_dev.pscmd = 0;
+
+                skb = bt_skb_alloc(sizeof(*cmd), GFP_ATOMIC);
+                if (skb == NULL) {
+                        BT_ERR("No free skb");
+                        return -ENOMEM;
+                }
+
+                cmd = (struct btmrvl_cmd *) skb_put(skb, sizeof(*cmd));
+                cmd->ocf_ogf = cpu_to_le16(hci_opcode_pack(OGF, BT_CMD_AUTO_SLEEP_MODE));
+                cmd->length = 1;
+
+                if (priv->btmrvl_dev.psmode)
+                        cmd->data[0] = BT_PS_ENABLE;
+                else
+                        cmd->data[0] = BT_PS_DISABLE;
+
+                bt_cb(skb)->pkt_type = MRVL_VENDOR_PKT;
+
+                skb->dev = (void *) priv->btmrvl_dev.hcidev;
+                skb_queue_head(&priv->adapter->tx_queue, skb);
+
+                BT_DBG("Queue PSMODE Command:%d", cmd->data[0]);
+        }
+
+        if (priv->btmrvl_dev.hscmd) {
+                priv->btmrvl_dev.hscmd = 0;
+
+                if (priv->btmrvl_dev.hsmode) {
+                        ret = btmrvl_enable_hs(priv);
+                } else {
+                        ret = priv->hw_wakeup_firmware(priv);
+                        priv->adapter->hs_state = HS_DEACTIVATED;
+                }
+        }
+
+        return ret;
+}
+
+static int btmrvl_tx_pkt(struct btmrvl_private *priv, struct sk_buff *skb)
+{
+        int ret = 0;
+
+        if (!skb || !skb->data)
+                return -EINVAL;
+
+        if (!skb->len || ((skb->len + BTM_HEADER_LEN) > BTM_UPLD_SIZE)) {
+                BT_ERR("Tx Error: Bad skb length %d : %d",
+                                                skb->len, BTM_UPLD_SIZE);
+                return -EINVAL;
+        }
+
+        if (skb_headroom(skb) < BTM_HEADER_LEN) {
+                struct sk_buff *tmp = skb;
+
+                skb = skb_realloc_headroom(skb, BTM_HEADER_LEN);
+                if (!skb) {
+                        BT_ERR("Tx Error: realloc_headroom failed %d",
+                                BTM_HEADER_LEN);
+                        skb = tmp;
+                        return -EINVAL;
+                }
+
+                kfree_skb(tmp);
+        }
+
+        skb_push(skb, BTM_HEADER_LEN);
+
+        /* header type: byte[3]
+         * HCI_COMMAND = 1, ACL_DATA = 2, SCO_DATA = 3, 0xFE = Vendor
+         * header length: byte[2][1][0]
+         */
+
+        skb->data[0] = (skb->len & 0x0000ff);
+        skb->data[1] = (skb->len & 0x00ff00) >> 8;
+        skb->data[2] = (skb->len & 0xff0000) >> 16;
+        skb->data[3] = bt_cb(skb)->pkt_type;
+
+        if (priv->hw_host_to_card)
+                ret = priv->hw_host_to_card(priv, skb->data, skb->len);
+
+        return ret;
+}
+
+static void btmrvl_init_adapter(struct btmrvl_private *priv)
+{
+        skb_queue_head_init(&priv->adapter->tx_queue);
+
+        priv->adapter->ps_state = PS_AWAKE;
+
+        init_waitqueue_head(&priv->adapter->cmd_wait_q);
+}
+
+static void btmrvl_free_adapter(struct btmrvl_private *priv)
+{
+        skb_queue_purge(&priv->adapter->tx_queue);
+
+        kfree(priv->adapter);
+
+        priv->adapter = NULL;
+}
+
+static int btmrvl_ioctl(struct hci_dev *hdev,
+                                unsigned int cmd, unsigned long arg)
+{
+        return -ENOIOCTLCMD;
+}
+
+static void btmrvl_destruct(struct hci_dev *hdev)
+{
+}
+
+static int btmrvl_send_frame(struct sk_buff *skb)
+{
+        struct hci_dev *hdev = (struct hci_dev *) skb->dev;
+        struct btmrvl_private *priv = NULL;
+
+        BT_DBG("type=%d, len=%d", skb->pkt_type, skb->len);
+
+        if (!hdev || !hdev->driver_data) {
+                BT_ERR("Frame for unknown HCI device");
+                return -ENODEV;
+        }
+
+        priv = (struct btmrvl_private *) hdev->driver_data;
+        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,
+                                                        skb->data, skb->len);
+                return -EBUSY;
+        }
+
+        switch (bt_cb(skb)->pkt_type) {
+        case HCI_COMMAND_PKT:
+                hdev->stat.cmd_tx++;
+                break;
+
+        case HCI_ACLDATA_PKT:
+                hdev->stat.acl_tx++;
+                break;
+
+        case HCI_SCODATA_PKT:
+                hdev->stat.sco_tx++;
+                break;
+        }
+
+        skb_queue_tail(&priv->adapter->tx_queue, skb);
+
+        wake_up_interruptible(&priv->main_thread.wait_q);
+
+        return 0;
+}
+
+static int btmrvl_flush(struct hci_dev *hdev)
+{
+        struct btmrvl_private *priv = hdev->driver_data;
+
+        skb_queue_purge(&priv->adapter->tx_queue);
+
+        return 0;
+}
+
+static int btmrvl_close(struct hci_dev *hdev)
+{
+        struct btmrvl_private *priv = hdev->driver_data;
+
+        if (!test_and_clear_bit(HCI_RUNNING, &hdev->flags))
+                return 0;
+
+        skb_queue_purge(&priv->adapter->tx_queue);
+
+        return 0;
+}
+
+static int btmrvl_open(struct hci_dev *hdev)
+{
+        set_bit(HCI_RUNNING, &hdev->flags);
+
+        return 0;
+}
+
+/*
+ * This function handles the event generated by firmware, rx data
+ * received from firmware, and tx data sent from kernel.
+ */
+static int btmrvl_service_main_thread(void *data)
+{
+        struct btmrvl_thread *thread = data;
+        struct btmrvl_private *priv = thread->priv;
+        struct btmrvl_adapter *adapter = priv->adapter;
+        wait_queue_t wait;
+        struct sk_buff *skb;
+        ulong flags;
+
+        init_waitqueue_entry(&wait, current);
+
+        current->flags |= PF_NOFREEZE;
+
+        for (;;) {
+                add_wait_queue(&thread->wait_q, &wait);
+
+                set_current_state(TASK_INTERRUPTIBLE);
+
+                if (adapter->wakeup_tries ||
+                                ((!adapter->int_count) &&
+                                (!priv->btmrvl_dev.tx_dnld_rdy ||
+                                skb_queue_empty(&adapter->tx_queue)))) {
+                        BT_DBG("main_thread is sleeping...");
+                        schedule();
+                }
+
+                set_current_state(TASK_RUNNING);
+
+                remove_wait_queue(&thread->wait_q, &wait);
+
+                BT_DBG("main_thread woke up");
+
+                if (kthread_should_stop()) {
+                        BT_DBG("main_thread: break from main thread");
+                        break;
+                }
+
+                spin_lock_irqsave(&priv->driver_lock, flags);
+                if (adapter->int_count) {
+                        adapter->int_count = 0;
+                } else if (adapter->ps_state == PS_SLEEP &&
+                                        !skb_queue_empty(&adapter->tx_queue)) {
+                        spin_unlock_irqrestore(&priv->driver_lock, flags);
+                        adapter->wakeup_tries++;
+                        priv->hw_wakeup_firmware(priv);
+                        continue;
+                }
+                spin_unlock_irqrestore(&priv->driver_lock, flags);
+
+                if (adapter->ps_state == PS_SLEEP)
+                        continue;
+
+                if (!priv->btmrvl_dev.tx_dnld_rdy)
+                        continue;
+
+                skb = skb_dequeue(&adapter->tx_queue);
+                if (skb) {
+                        if (btmrvl_tx_pkt(priv, skb))
+                                priv->btmrvl_dev.hcidev->stat.err_tx++;
+                        else
+                                priv->btmrvl_dev.hcidev->stat.byte_tx += skb->len;
+
+                        kfree_skb(skb);
+                }
+        }
+
+        return 0;
+}
+
+struct btmrvl_private *btmrvl_add_card(void *card)
+{
+        struct hci_dev *hdev = NULL;
+        struct btmrvl_private *priv;
+        int ret;
+
+        priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+        if (!priv) {
+                BT_ERR("Can not allocate priv");
+                goto err_priv;
+        }
+
+        priv->adapter = kzalloc(sizeof(*priv->adapter), GFP_KERNEL);
+        if (!priv->adapter) {
+                BT_ERR("Allocate buffer for btmrvl_adapter failed!");
+                goto err_adapter;
+        }
+
+        btmrvl_init_adapter(priv);
+
+        hdev = hci_alloc_dev();
+        if (!hdev) {
+                BT_ERR("Can not allocate HCI device");
+                goto err_hdev;
+        }
+
+        BT_DBG("Starting kthread...");
+        priv->main_thread.priv = priv;
+        spin_lock_init(&priv->driver_lock);
+
+        init_waitqueue_head(&priv->main_thread.wait_q);
+        priv->main_thread.task = kthread_run(btmrvl_service_main_thread,
+                                &priv->main_thread, "btmrvl_main_service");
+
+        priv->btmrvl_dev.hcidev = hdev;
+        priv->btmrvl_dev.card = card;
+
+        hdev->driver_data = priv;
+
+        priv->btmrvl_dev.tx_dnld_rdy = true;
+
+        hdev->type = HCI_SDIO;
+        hdev->open = btmrvl_open;
+        hdev->close = btmrvl_close;
+        hdev->flush = btmrvl_flush;
+        hdev->send = btmrvl_send_frame;
+        hdev->destruct = btmrvl_destruct;
+        hdev->ioctl = btmrvl_ioctl;
+        hdev->owner = THIS_MODULE;
+
+        ret = hci_register_dev(hdev);
+        if (ret < 0) {
+                BT_ERR("Can not register HCI device");
+                goto err_hci_register_dev;
+        }
+
+#ifdef CONFIG_DEBUG_FS
+        btmrvl_debugfs_init(hdev);
+#endif
+
+        return priv;
+
+err_hci_register_dev:
+        /* Stop the thread servicing the interrupts */
+        kthread_stop(priv->main_thread.task);
+
+        hci_free_dev(hdev);
+
+err_hdev:
+        btmrvl_free_adapter(priv);
+
+err_adapter:
+        kfree(priv);
+
+err_priv:
+        return NULL;
+}
+EXPORT_SYMBOL_GPL(btmrvl_add_card);
+
+int btmrvl_remove_card(struct btmrvl_private *priv)
+{
+        struct hci_dev *hdev;
+
+        hdev = priv->btmrvl_dev.hcidev;
+
+        wake_up_interruptible(&priv->adapter->cmd_wait_q);
+
+        kthread_stop(priv->main_thread.task);
+
+#ifdef CONFIG_DEBUG_FS
+        btmrvl_debugfs_remove(hdev);
+#endif
+
+        hci_unregister_dev(hdev);
+
+        hci_free_dev(hdev);
+
+        priv->btmrvl_dev.hcidev = NULL;
+
+        btmrvl_free_adapter(priv);
+
+        kfree(priv);
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(btmrvl_remove_card);
+
+MODULE_AUTHOR("Marvell International Ltd.");
+MODULE_DESCRIPTION("Marvell Bluetooth driver ver " VERSION);
+MODULE_VERSION(VERSION);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/bluetooth/btmrvl_sdio.c b/drivers/bluetooth/btmrvl_sdio.c
new file mode 100644
index 000000000000..5b33b85790f2
--- /dev/null
+++ b/drivers/bluetooth/btmrvl_sdio.c
@@ -0,0 +1,1003 @@
+/**
+ * Marvell BT-over-SDIO driver: SDIO interface related functions.
+ *
+ * Copyright (C) 2009, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ **/
+
+#include <linux/firmware.h>
+
+#include <linux/mmc/sdio_ids.h>
+#include <linux/mmc/sdio_func.h>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+
+#include "btmrvl_drv.h"
+#include "btmrvl_sdio.h"
+
+#define VERSION "1.0"
+
+/* The btmrvl_sdio_remove() callback function is called
+ * when user removes this module from kernel space or ejects
+ * the card from the slot. The driver handles these 2 cases
+ * differently.
+ * If the user is removing the module, a MODULE_SHUTDOWN_REQ
+ * command is sent to firmware and interrupt will be disabled.
+ * If the card is removed, there is no need to send command
+ * or disable interrupt.
+ *
+ * The variable 'user_rmmod' is used to distinguish these two
+ * scenarios. This flag is initialized as FALSE in case the card
+ * is removed, and will be set to TRUE for module removal when
+ * module_exit function is called.
+ */
+static u8 user_rmmod;
+
+static const struct btmrvl_sdio_device btmrvl_sdio_sd6888 = {
+        .helper         = "sd8688_helper.bin",
+        .firmware       = "sd8688.bin",
+};
+
+static const struct sdio_device_id btmrvl_sdio_ids[] = {
+        /* Marvell SD8688 Bluetooth device */
+        { SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, 0x9105),
+                        .driver_data = (unsigned long) &btmrvl_sdio_sd6888 },
+
+        { }     /* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(sdio, btmrvl_sdio_ids);
+
+static int btmrvl_sdio_get_rx_unit(struct btmrvl_sdio_card *card)
+{
+        u8 reg;
+        int ret;
+
+        reg = sdio_readb(card->func, CARD_RX_UNIT_REG, &ret);
+        if (!ret)
+                card->rx_unit = reg;
+
+        return ret;
+}
+
+static int btmrvl_sdio_read_fw_status(struct btmrvl_sdio_card *card, u16 *dat)
+{
+        u8 fws0, fws1;
+        int ret;
+
+        *dat = 0;
+
+        fws0 = sdio_readb(card->func, CARD_FW_STATUS0_REG, &ret);
+
+        if (!ret)
+                fws1 = sdio_readb(card->func, CARD_FW_STATUS1_REG, &ret);
+
+        if (ret)
+                return -EIO;
+
+        *dat = (((u16) fws1) << 8) | fws0;
+
+        return 0;
+}
+
+static int btmrvl_sdio_read_rx_len(struct btmrvl_sdio_card *card, u16 *dat)
+{
+        u8 reg;
+        int ret;
+
+        reg = sdio_readb(card->func, CARD_RX_LEN_REG, &ret);
+        if (!ret)
+                *dat = (u16) reg << card->rx_unit;
+
+        return ret;
+}
+
+static int btmrvl_sdio_enable_host_int_mask(struct btmrvl_sdio_card *card,
+                                                                u8 mask)
+{
+        int ret;
+
+        sdio_writeb(card->func, mask, HOST_INT_MASK_REG, &ret);
+        if (ret) {
+                BT_ERR("Unable to enable the host interrupt!");
+                ret = -EIO;
+        }
+
+        return ret;
+}
+
+static int btmrvl_sdio_disable_host_int_mask(struct btmrvl_sdio_card *card,
+                                                                u8 mask)
+{
+        u8 host_int_mask;
+        int ret;
+
+        host_int_mask = sdio_readb(card->func, HOST_INT_MASK_REG, &ret);
+        if (ret)
+                return -EIO;
+
+        host_int_mask &= ~mask;
+
+        sdio_writeb(card->func, host_int_mask, HOST_INT_MASK_REG, &ret);
+        if (ret < 0) {
+                BT_ERR("Unable to disable the host interrupt!");
+                return -EIO;
+        }
+
+        return 0;
+}
+
+static int btmrvl_sdio_poll_card_status(struct btmrvl_sdio_card *card, u8 bits)
+{
+        unsigned int tries;
+        u8 status;
+        int ret;
+
+        for (tries = 0; tries < MAX_POLL_TRIES * 1000; tries++) {
+                status = sdio_readb(card->func, CARD_STATUS_REG, &ret);
+                if (ret)
+                        goto failed;
+                if ((status & bits) == bits)
+                        return ret;
+
+                udelay(1);
+        }
+
+        ret = -ETIMEDOUT;
+
+failed:
+        BT_ERR("FAILED! ret=%d", ret);
+
+        return ret;
+}
+
+static int btmrvl_sdio_verify_fw_download(struct btmrvl_sdio_card *card,
+                                                                int pollnum)
+{
+        int ret = -ETIMEDOUT;
+        u16 firmwarestat;
+        unsigned int tries;
+
+         /* Wait for firmware to become ready */
+        for (tries = 0; tries < pollnum; tries++) {
+                if (btmrvl_sdio_read_fw_status(card, &firmwarestat) < 0)
+                        continue;
+
+                if (firmwarestat == FIRMWARE_READY) {
+                        ret = 0;
+                        break;
+                } else {
+                        msleep(10);
+                }
+        }
+
+        return ret;
+}
+
+static int btmrvl_sdio_download_helper(struct btmrvl_sdio_card *card)
+{
+        const struct firmware *fw_helper = NULL;
+        const u8 *helper = NULL;
+        int ret;
+        void *tmphlprbuf = NULL;
+        int tmphlprbufsz, hlprblknow, helperlen;
+        u8 *helperbuf;
+        u32 tx_len;
+
+        ret = request_firmware(&fw_helper, card->helper,
+                                                &card->func->dev);
+        if ((ret < 0) || !fw_helper) {
+                BT_ERR("request_firmware(helper) failed, error code = %d",
+                                                                        ret);
+                ret = -ENOENT;
+                goto done;
+        }
+
+        helper = fw_helper->data;
+        helperlen = fw_helper->size;
+
+        BT_DBG("Downloading helper image (%d bytes), block size %d bytes",
+                                                helperlen, SDIO_BLOCK_SIZE);
+
+        tmphlprbufsz = ALIGN_SZ(BTM_UPLD_SIZE, BTSDIO_DMA_ALIGN);
+
+        tmphlprbuf = kmalloc(tmphlprbufsz, GFP_KERNEL);
+        if (!tmphlprbuf) {
+                BT_ERR("Unable to allocate buffer for helper."
+                        " Terminating download");
+                ret = -ENOMEM;
+                goto done;
+        }
+
+        memset(tmphlprbuf, 0, tmphlprbufsz);
+
+        helperbuf = (u8 *) ALIGN_ADDR(tmphlprbuf, BTSDIO_DMA_ALIGN);
+
+        /* Perform helper data transfer */
+        tx_len = (FIRMWARE_TRANSFER_NBLOCK * SDIO_BLOCK_SIZE)
+                        - SDIO_HEADER_LEN;
+        hlprblknow = 0;
+
+        do {
+                ret = btmrvl_sdio_poll_card_status(card,
+                                            CARD_IO_READY | DN_LD_CARD_RDY);
+                if (ret < 0) {
+                        BT_ERR("Helper download poll status timeout @ %d",
+                                hlprblknow);
+                        goto done;
+                }
+
+                /* Check if there is more data? */
+                if (hlprblknow >= helperlen)
+                        break;
+
+                if (helperlen - hlprblknow < tx_len)
+                        tx_len = helperlen - hlprblknow;
+
+                /* Little-endian */
+                helperbuf[0] = ((tx_len & 0x000000ff) >> 0);
+                helperbuf[1] = ((tx_len & 0x0000ff00) >> 8);
+                helperbuf[2] = ((tx_len & 0x00ff0000) >> 16);
+                helperbuf[3] = ((tx_len & 0xff000000) >> 24);
+
+                memcpy(&helperbuf[SDIO_HEADER_LEN], &helper[hlprblknow],
+                                tx_len);
+
+                /* Now send the data */
+                ret = sdio_writesb(card->func, card->ioport, helperbuf,
+                                FIRMWARE_TRANSFER_NBLOCK * SDIO_BLOCK_SIZE);
+                if (ret < 0) {
+                        BT_ERR("IO error during helper download @ %d",
+                                hlprblknow);
+                        goto done;
+                }
+
+                hlprblknow += tx_len;
+        } while (true);
+
+        BT_DBG("Transferring helper image EOF block");
+
+        memset(helperbuf, 0x0, SDIO_BLOCK_SIZE);
+
+        ret = sdio_writesb(card->func, card->ioport, helperbuf,
+                                                        SDIO_BLOCK_SIZE);
+        if (ret < 0) {
+                BT_ERR("IO error in writing helper image EOF block");
+                goto done;
+        }
+
+        ret = 0;
+
+done:
+        kfree(tmphlprbuf);
+        if (fw_helper)
+                release_firmware(fw_helper);
+
+        return ret;
+}
+
+static int btmrvl_sdio_download_fw_w_helper(struct btmrvl_sdio_card *card)
+{
+        const struct firmware *fw_firmware = NULL;
+        const u8 *firmware = NULL;
+        int firmwarelen, tmpfwbufsz, ret;
+        unsigned int tries, offset;
+        u8 base0, base1;
+        void *tmpfwbuf = NULL;
+        u8 *fwbuf;
+        u16 len;
+        int txlen = 0, tx_blocks = 0, count = 0;
+
+        ret = request_firmware(&fw_firmware, card->firmware,
+                                                        &card->func->dev);
+        if ((ret < 0) || !fw_firmware) {
+                BT_ERR("request_firmware(firmware) failed, error code = %d",
+                                                                        ret);
+                ret = -ENOENT;
+                goto done;
+        }
+
+        firmware = fw_firmware->data;
+        firmwarelen = fw_firmware->size;
+
+        BT_DBG("Downloading FW image (%d bytes)", firmwarelen);
+
+        tmpfwbufsz = ALIGN_SZ(BTM_UPLD_SIZE, BTSDIO_DMA_ALIGN);
+        tmpfwbuf = kmalloc(tmpfwbufsz, GFP_KERNEL);
+        if (!tmpfwbuf) {
+                BT_ERR("Unable to allocate buffer for firmware."
+                       " Terminating download");
+                ret = -ENOMEM;
+                goto done;
+        }
+
+        memset(tmpfwbuf, 0, tmpfwbufsz);
+
+        /* Ensure aligned firmware buffer */
+        fwbuf = (u8 *) ALIGN_ADDR(tmpfwbuf, BTSDIO_DMA_ALIGN);
+
+        /* Perform firmware data transfer */
+        offset = 0;
+        do {
+                ret = btmrvl_sdio_poll_card_status(card,
+                                        CARD_IO_READY | DN_LD_CARD_RDY);
+                if (ret < 0) {
+                        BT_ERR("FW download with helper poll status"
+                                                " timeout @ %d", offset);
+                        goto done;
+                }
+
+                /* Check if there is more data ? */
+                if (offset >= firmwarelen)
+                        break;
+
+                for (tries = 0; tries < MAX_POLL_TRIES; tries++) {
+                        base0 = sdio_readb(card->func,
+                                        SQ_READ_BASE_ADDRESS_A0_REG, &ret);
+                        if (ret) {
+                                BT_ERR("BASE0 register read failed:"
+                                        " base0 = 0x%04X(%d)."
+                                        " Terminating download",
+                                        base0, base0);
+                                ret = -EIO;
+                                goto done;
+                        }
+                        base1 = sdio_readb(card->func,
+                                        SQ_READ_BASE_ADDRESS_A1_REG, &ret);
+                        if (ret) {
+                                BT_ERR("BASE1 register read failed:"
+                                        " base1 = 0x%04X(%d)."
+                                        " Terminating download",
+                                        base1, base1);
+                                ret = -EIO;
+                                goto done;
+                        }
+
+                        len = (((u16) base1) << 8) | base0;
+                        if (len)
+                                break;
+
+                        udelay(10);
+                }
+
+                if (!len)
+                        break;
+                else if (len > BTM_UPLD_SIZE) {
+                        BT_ERR("FW download failure @%d, invalid length %d",
+                                                                offset, len);
+                        ret = -EINVAL;
+                        goto done;
+                }
+
+                txlen = len;
+
+                if (len & BIT(0)) {
+                        count++;
+                        if (count > MAX_WRITE_IOMEM_RETRY) {
+                                BT_ERR("FW download failure @%d, "
+                                        "over max retry count", offset);
+                                ret = -EIO;
+                                goto done;
+                        }
+                        BT_ERR("FW CRC error indicated by the helper: "
+                                "len = 0x%04X, txlen = %d", len, txlen);
+                        len &= ~BIT(0);
+                        /* Set txlen to 0 so as to resend from same offset */
+                        txlen = 0;
+                } else {
+                        count = 0;
+
+                        /* Last block ? */
+                        if (firmwarelen - offset < txlen)
+                                txlen = firmwarelen - offset;
+
+                        tx_blocks =
+                            (txlen + SDIO_BLOCK_SIZE - 1) / SDIO_BLOCK_SIZE;
+
+                        memcpy(fwbuf, &firmware[offset], txlen);
+                }
+
+                ret = sdio_writesb(card->func, card->ioport, fwbuf,
+                                        tx_blocks * SDIO_BLOCK_SIZE);
+
+                if (ret < 0) {
+                        BT_ERR("FW download, writesb(%d) failed @%d",
+                                                        count, offset);
+                        sdio_writeb(card->func, HOST_CMD53_FIN, CONFIG_REG,
+                                                                        &ret);
+                        if (ret)
+                                BT_ERR("writeb failed (CFG)");
+                }
+
+                offset += txlen;
+        } while (true);
+
+        BT_DBG("FW download over, size %d bytes", offset);
+
+        ret = 0;
+
+done:
+        kfree(tmpfwbuf);
+
+        if (fw_firmware)
+                release_firmware(fw_firmware);
+
+        return ret;
+}
+
+static int btmrvl_sdio_card_to_host(struct btmrvl_private *priv)
+{
+        u16 buf_len = 0;
+        int ret, buf_block_len, blksz;
+        struct sk_buff *skb = NULL;
+        u32 type;
+        u8 *payload = NULL;
+        struct hci_dev *hdev = priv->btmrvl_dev.hcidev;
+        struct btmrvl_sdio_card *card = priv->btmrvl_dev.card;
+
+        if (!card || !card->func) {
+                BT_ERR("card or function is NULL!");
+                ret = -EINVAL;
+                goto exit;
+        }
+
+        /* Read the length of data to be transferred */
+        ret = btmrvl_sdio_read_rx_len(card, &buf_len);
+        if (ret < 0) {
+                BT_ERR("read rx_len failed");
+                ret = -EIO;
+                goto exit;
+        }
+
+        blksz = SDIO_BLOCK_SIZE;
+        buf_block_len = (buf_len + blksz - 1) / blksz;
+
+        if (buf_len <= SDIO_HEADER_LEN
+                        || (buf_block_len * blksz) > ALLOC_BUF_SIZE) {
+                BT_ERR("invalid packet length: %d", buf_len);
+                ret = -EINVAL;
+                goto exit;
+        }
+
+        /* Allocate buffer */
+        skb = bt_skb_alloc(buf_block_len * blksz + BTSDIO_DMA_ALIGN,
+                                                                GFP_ATOMIC);
+        if (skb == NULL) {
+                BT_ERR("No free skb");
+                goto exit;
+        }
+
+        if ((unsigned long) skb->data & (BTSDIO_DMA_ALIGN - 1)) {
+                skb_put(skb, (unsigned long) skb->data &
+                                        (BTSDIO_DMA_ALIGN - 1));
+                skb_pull(skb, (unsigned long) skb->data &
+                                        (BTSDIO_DMA_ALIGN - 1));
+        }
+
+        payload = skb->data;
+
+        ret = sdio_readsb(card->func, payload, card->ioport,
+                          buf_block_len * blksz);
+        if (ret < 0) {
+                BT_ERR("readsb failed: %d", ret);
+                ret = -EIO;
+                goto exit;
+        }
+
+        /* This is SDIO specific header length: byte[2][1][0], type: byte[3]
+         * (HCI_COMMAND = 1, ACL_DATA = 2, SCO_DATA = 3, 0xFE = Vendor)
+         */
+
+        buf_len = payload[0];
+        buf_len |= (u16) payload[1] << 8;
+        type = payload[3];
+
+        switch (type) {
+        case HCI_ACLDATA_PKT:
+        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)
+                        btmrvl_check_evtpkt(priv, skb);
+
+                hci_recv_frame(skb);
+                hdev->stat.byte_rx += buf_len;
+                break;
+
+        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);
+
+                hdev->stat.byte_rx += buf_len;
+                break;
+
+        default:
+                BT_ERR("Unknow packet type:%d", type);
+                print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, payload,
+                                                blksz * buf_block_len);
+
+                kfree_skb(skb);
+                skb = NULL;
+                break;
+        }
+
+exit:
+        if (ret) {
+                hdev->stat.err_rx++;
+                if (skb)
+                        kfree_skb(skb);
+        }
+
+        return ret;
+}
+
+static int btmrvl_sdio_get_int_status(struct btmrvl_private *priv, u8 * ireg)
+{
+        int ret;
+        u8 sdio_ireg = 0;
+        struct btmrvl_sdio_card *card = priv->btmrvl_dev.card;
+
+        *ireg = 0;
+
+        sdio_ireg = sdio_readb(card->func, HOST_INTSTATUS_REG, &ret);
+        if (ret) {
+                BT_ERR("sdio_readb: read int status register failed");
+                ret = -EIO;
+                goto done;
+        }
+
+        if (sdio_ireg != 0) {
+                /*
+                 * DN_LD_HOST_INT_STATUS and/or UP_LD_HOST_INT_STATUS
+                 * Clear the interrupt status register and re-enable the
+                 * interrupt.
+                 */
+                BT_DBG("sdio_ireg = 0x%x", sdio_ireg);
+
+                sdio_writeb(card->func, ~(sdio_ireg) & (DN_LD_HOST_INT_STATUS |
+                                                        UP_LD_HOST_INT_STATUS),
+                            HOST_INTSTATUS_REG, &ret);
+                if (ret) {
+                        BT_ERR("sdio_writeb: clear int status register "
+                                "failed");
+                        ret = -EIO;
+                        goto done;
+                }
+        }
+
+        if (sdio_ireg & DN_LD_HOST_INT_STATUS) {
+                if (priv->btmrvl_dev.tx_dnld_rdy)
+                        BT_DBG("tx_done already received: "
+                                " int_status=0x%x", sdio_ireg);
+                else
+                        priv->btmrvl_dev.tx_dnld_rdy = true;
+        }
+
+        if (sdio_ireg & UP_LD_HOST_INT_STATUS)
+                btmrvl_sdio_card_to_host(priv);
+
+        *ireg = sdio_ireg;
+
+        ret = 0;
+
+done:
+        return ret;
+}
+
+static void btmrvl_sdio_interrupt(struct sdio_func *func)
+{
+        struct btmrvl_private *priv;
+        struct hci_dev *hcidev;
+        struct btmrvl_sdio_card *card;
+        u8 ireg = 0;
+
+        card = sdio_get_drvdata(func);
+        if (card && card->priv) {
+                priv = card->priv;
+                hcidev = priv->btmrvl_dev.hcidev;
+
+                if (btmrvl_sdio_get_int_status(priv, &ireg))
+                        BT_ERR("reading HOST_INT_STATUS_REG failed");
+                else
+                        BT_DBG("HOST_INT_STATUS_REG %#x", ireg);
+
+                btmrvl_interrupt(priv);
+        }
+}
+
+static int btmrvl_sdio_register_dev(struct btmrvl_sdio_card *card)
+{
+        struct sdio_func *func;
+        u8 reg;
+        int ret = 0;
+
+        if (!card || !card->func) {
+                BT_ERR("Error: card or function is NULL!");
+                ret = -EINVAL;
+                goto failed;
+        }
+
+        func = card->func;
+
+        sdio_claim_host(func);
+
+        ret = sdio_enable_func(func);
+        if (ret) {
+                BT_ERR("sdio_enable_func() failed: ret=%d", ret);
+                ret = -EIO;
+                goto release_host;
+        }
+
+        ret = sdio_claim_irq(func, btmrvl_sdio_interrupt);
+        if (ret) {
+                BT_ERR("sdio_claim_irq failed: ret=%d", ret);
+                ret = -EIO;
+                goto disable_func;
+        }
+
+        ret = sdio_set_block_size(card->func, SDIO_BLOCK_SIZE);
+        if (ret) {
+                BT_ERR("cannot set SDIO block size");
+                ret = -EIO;
+                goto release_irq;
+        }
+
+        reg = sdio_readb(func, IO_PORT_0_REG, &ret);
+        if (ret < 0) {
+                ret = -EIO;
+                goto release_irq;
+        }
+
+        card->ioport = reg;
+
+        reg = sdio_readb(func, IO_PORT_1_REG, &ret);
+        if (ret < 0) {
+                ret = -EIO;
+                goto release_irq;
+        }
+
+        card->ioport |= (reg << 8);
+
+        reg = sdio_readb(func, IO_PORT_2_REG, &ret);
+        if (ret < 0) {
+                ret = -EIO;
+                goto release_irq;
+        }
+
+        card->ioport |= (reg << 16);
+
+        BT_DBG("SDIO FUNC%d IO port: 0x%x", func->num, card->ioport);
+
+        sdio_set_drvdata(func, card);
+
+        sdio_release_host(func);
+
+        return 0;
+
+release_irq:
+        sdio_release_irq(func);
+
+disable_func:
+        sdio_disable_func(func);
+
+release_host:
+        sdio_release_host(func);
+
+failed:
+        return ret;
+}
+
+static int btmrvl_sdio_unregister_dev(struct btmrvl_sdio_card *card)
+{
+        if (card && card->func) {
+                sdio_claim_host(card->func);
+                sdio_release_irq(card->func);
+                sdio_disable_func(card->func);
+                sdio_release_host(card->func);
+                sdio_set_drvdata(card->func, NULL);
+        }
+
+        return 0;
+}
+
+static int btmrvl_sdio_enable_host_int(struct btmrvl_sdio_card *card)
+{
+        int ret;
+
+        if (!card || !card->func)
+                return -EINVAL;
+
+        sdio_claim_host(card->func);
+
+        ret = btmrvl_sdio_enable_host_int_mask(card, HIM_ENABLE);
+
+        btmrvl_sdio_get_rx_unit(card);
+
+        sdio_release_host(card->func);
+
+        return ret;
+}
+
+static int btmrvl_sdio_disable_host_int(struct btmrvl_sdio_card *card)
+{
+        int ret;
+
+        if (!card || !card->func)
+                return -EINVAL;
+
+        sdio_claim_host(card->func);
+
+        ret = btmrvl_sdio_disable_host_int_mask(card, HIM_DISABLE);
+
+        sdio_release_host(card->func);
+
+        return ret;
+}
+
+static int btmrvl_sdio_host_to_card(struct btmrvl_private *priv,
+                                u8 *payload, u16 nb)
+{
+        struct btmrvl_sdio_card *card = priv->btmrvl_dev.card;
+        int ret = 0;
+        int buf_block_len;
+        int blksz;
+        int i = 0;
+        u8 *buf = NULL;
+        void *tmpbuf = NULL;
+        int tmpbufsz;
+
+        if (!card || !card->func) {
+                BT_ERR("card or function is NULL!");
+                return -EINVAL;
+        }
+
+        buf = payload;
+        if ((unsigned long) payload & (BTSDIO_DMA_ALIGN - 1)) {
+                tmpbufsz = ALIGN_SZ(nb, BTSDIO_DMA_ALIGN);
+                tmpbuf = kzalloc(tmpbufsz, GFP_KERNEL);
+                if (!tmpbuf)
+                        return -ENOMEM;
+                buf = (u8 *) ALIGN_ADDR(tmpbuf, BTSDIO_DMA_ALIGN);
+                memcpy(buf, payload, nb);
+        }
+
+        blksz = SDIO_BLOCK_SIZE;
+        buf_block_len = (nb + blksz - 1) / blksz;
+
+        sdio_claim_host(card->func);
+
+        do {
+                /* Transfer data to card */
+                ret = sdio_writesb(card->func, card->ioport, buf,
+                                   buf_block_len * blksz);
+                if (ret < 0) {
+                        i++;
+                        BT_ERR("i=%d writesb failed: %d", i, ret);
+                        print_hex_dump_bytes("", DUMP_PREFIX_OFFSET,
+                                                payload, nb);
+                        ret = -EIO;
+                        if (i > MAX_WRITE_IOMEM_RETRY)
+                                goto exit;
+                }
+        } while (ret);
+
+        priv->btmrvl_dev.tx_dnld_rdy = false;
+
+exit:
+        sdio_release_host(card->func);
+
+        return ret;
+}
+
+static int btmrvl_sdio_download_fw(struct btmrvl_sdio_card *card)
+{
+        int ret = 0;
+
+        if (!card || !card->func) {
+                BT_ERR("card or function is NULL!");
+                return -EINVAL;
+        }
+        sdio_claim_host(card->func);
+
+        if (!btmrvl_sdio_verify_fw_download(card, 1)) {
+                BT_DBG("Firmware already downloaded!");
+                goto done;
+        }
+
+        ret = btmrvl_sdio_download_helper(card);
+        if (ret) {
+                BT_ERR("Failed to download helper!");
+                ret = -EIO;
+                goto done;
+        }
+
+        if (btmrvl_sdio_download_fw_w_helper(card)) {
+                BT_ERR("Failed to download firmware!");
+                ret = -EIO;
+                goto done;
+        }
+
+        if (btmrvl_sdio_verify_fw_download(card, MAX_POLL_TRIES)) {
+                BT_ERR("FW failed to be active in time!");
+                ret = -ETIMEDOUT;
+                goto done;
+        }
+
+done:
+        sdio_release_host(card->func);
+
+        return ret;
+}
+
+static int btmrvl_sdio_wakeup_fw(struct btmrvl_private *priv)
+{
+        struct btmrvl_sdio_card *card = priv->btmrvl_dev.card;
+        int ret = 0;
+
+        if (!card || !card->func) {
+                BT_ERR("card or function is NULL!");
+                return -EINVAL;
+        }
+
+        sdio_claim_host(card->func);
+
+        sdio_writeb(card->func, HOST_POWER_UP, CONFIG_REG, &ret);
+
+        sdio_release_host(card->func);
+
+        BT_DBG("wake up firmware");
+
+        return ret;
+}
+
+static int btmrvl_sdio_probe(struct sdio_func *func,
+                                        const struct sdio_device_id *id)
+{
+        int ret = 0;
+        struct btmrvl_private *priv = NULL;
+        struct btmrvl_sdio_card *card = NULL;
+
+        BT_INFO("vendor=0x%x, device=0x%x, class=%d, fn=%d",
+                        id->vendor, id->device, id->class, func->num);
+
+        card = kzalloc(sizeof(*card), GFP_KERNEL);
+        if (!card) {
+                ret = -ENOMEM;
+                goto done;
+        }
+
+        card->func = func;
+
+        if (id->driver_data) {
+                struct btmrvl_sdio_device *data = (void *) id->driver_data;
+                card->helper   = data->helper;
+                card->firmware = data->firmware;
+        }
+
+        if (btmrvl_sdio_register_dev(card) < 0) {
+                BT_ERR("Failed to register BT device!");
+                ret = -ENODEV;
+                goto free_card;
+        }
+
+        /* Disable the interrupts on the card */
+        btmrvl_sdio_disable_host_int(card);
+
+        if (btmrvl_sdio_download_fw(card)) {
+                BT_ERR("Downloading firmware failed!");
+                ret = -ENODEV;
+                goto unreg_dev;
+        }
+
+        msleep(100);
+
+        btmrvl_sdio_enable_host_int(card);
+
+        priv = btmrvl_add_card(card);
+        if (!priv) {
+                BT_ERR("Initializing card failed!");
+                ret = -ENODEV;
+                goto disable_host_int;
+        }
+
+        card->priv = priv;
+
+        /* Initialize the interface specific function pointers */
+        priv->hw_host_to_card = btmrvl_sdio_host_to_card;
+        priv->hw_wakeup_firmware = btmrvl_sdio_wakeup_fw;
+
+        btmrvl_send_module_cfg_cmd(priv, MODULE_BRINGUP_REQ);
+
+        return 0;
+
+disable_host_int:
+        btmrvl_sdio_disable_host_int(card);
+unreg_dev:
+        btmrvl_sdio_unregister_dev(card);
+free_card:
+        kfree(card);
+done:
+        return ret;
+}
+
+static void btmrvl_sdio_remove(struct sdio_func *func)
+{
+        struct btmrvl_sdio_card *card;
+
+        if (func) {
+                card = sdio_get_drvdata(func);
+                if (card) {
+                        /* Send SHUTDOWN command & disable interrupt
+                         * if user removes the module.
+                         */
+                        if (user_rmmod) {
+                                btmrvl_send_module_cfg_cmd(card->priv,
+                                                        MODULE_SHUTDOWN_REQ);
+                                btmrvl_sdio_disable_host_int(card);
+                        }
+                        BT_DBG("unregester dev");
+                        btmrvl_sdio_unregister_dev(card);
+                        btmrvl_remove_card(card->priv);
+                        kfree(card);
+                }
+        }
+}
+
+static struct sdio_driver bt_mrvl_sdio = {
+        .name           = "btmrvl_sdio",
+        .id_table       = btmrvl_sdio_ids,
+        .probe          = btmrvl_sdio_probe,
+        .remove         = btmrvl_sdio_remove,
+};
+
+static int btmrvl_sdio_init_module(void)
+{
+        if (sdio_register_driver(&bt_mrvl_sdio) != 0) {
+                BT_ERR("SDIO Driver Registration Failed");
+                return -ENODEV;
+        }
+
+        /* Clear the flag in case user removes the card. */
+        user_rmmod = 0;
+
+        return 0;
+}
+
+static void btmrvl_sdio_exit_module(void)
+{
+        /* Set the flag as user is removing this module. */
+        user_rmmod = 1;
+
+        sdio_unregister_driver(&bt_mrvl_sdio);
+}
+
+module_init(btmrvl_sdio_init_module);
+module_exit(btmrvl_sdio_exit_module);
+
+MODULE_AUTHOR("Marvell International Ltd.");
+MODULE_DESCRIPTION("Marvell BT-over-SDIO driver ver " VERSION);
+MODULE_VERSION(VERSION);
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/bluetooth/btmrvl_sdio.h b/drivers/bluetooth/btmrvl_sdio.h
new file mode 100644
index 000000000000..27329f107e5a
--- /dev/null
+++ b/drivers/bluetooth/btmrvl_sdio.h
@@ -0,0 +1,108 @@
+/**
+ * Marvell BT-over-SDIO driver: SDIO interface related definitions
+ *
+ * Copyright (C) 2009, Marvell International Ltd.
+ *
+ * This software file (the "File") is distributed by Marvell International
+ * Ltd. under the terms of the GNU General Public License Version 2, June 1991
+ * (the "License").  You may use, redistribute and/or modify this File in
+ * accordance with the terms and conditions of the License, a copy of which
+ * is available by writing to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA or on the
+ * worldwide web at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt.
+ *
+ *
+ * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE
+ * ARE EXPRESSLY DISCLAIMED.  The License provides additional details about
+ * this warranty disclaimer.
+ *
+ **/
+
+#define SDIO_HEADER_LEN                 4
+
+/* SD block size can not bigger than 64 due to buf size limit in firmware */
+/* define SD block size for data Tx/Rx */
+#define SDIO_BLOCK_SIZE                 64
+
+/* Number of blocks for firmware transfer */
+#define FIRMWARE_TRANSFER_NBLOCK        2
+
+/* This is for firmware specific length */
+#define FW_EXTRA_LEN                    36
+
+#define MRVDRV_SIZE_OF_CMD_BUFFER       (2 * 1024)
+
+#define MRVDRV_BT_RX_PACKET_BUFFER_SIZE \
+        (HCI_MAX_FRAME_SIZE + FW_EXTRA_LEN)
+
+#define ALLOC_BUF_SIZE  (((max_t (int, MRVDRV_BT_RX_PACKET_BUFFER_SIZE, \
+                        MRVDRV_SIZE_OF_CMD_BUFFER) + SDIO_HEADER_LEN \
+                        + SDIO_BLOCK_SIZE - 1) / SDIO_BLOCK_SIZE) \
+                        * SDIO_BLOCK_SIZE)
+
+/* The number of times to try when polling for status */
+#define MAX_POLL_TRIES                  100
+
+/* Max retry number of CMD53 write */
+#define MAX_WRITE_IOMEM_RETRY           2
+
+/* Host Control Registers */
+#define IO_PORT_0_REG                   0x00
+#define IO_PORT_1_REG                   0x01
+#define IO_PORT_2_REG                   0x02
+
+#define CONFIG_REG                      0x03
+#define HOST_POWER_UP                   BIT(1)
+#define HOST_CMD53_FIN                  BIT(2)
+
+#define HOST_INT_MASK_REG               0x04
+#define HIM_DISABLE                     0xff
+#define HIM_ENABLE                      (BIT(0) | BIT(1))
+
+#define HOST_INTSTATUS_REG              0x05
+#define UP_LD_HOST_INT_STATUS           BIT(0)
+#define DN_LD_HOST_INT_STATUS           BIT(1)
+
+/* Card Control Registers */
+#define SQ_READ_BASE_ADDRESS_A0_REG     0x10
+#define SQ_READ_BASE_ADDRESS_A1_REG     0x11
+
+#define CARD_STATUS_REG                 0x20
+#define DN_LD_CARD_RDY                  BIT(0)
+#define CARD_IO_READY                   BIT(3)
+
+#define CARD_FW_STATUS0_REG             0x40
+#define CARD_FW_STATUS1_REG             0x41
+#define FIRMWARE_READY                  0xfedc
+
+#define CARD_RX_LEN_REG                 0x42
+#define CARD_RX_UNIT_REG                0x43
+
+
+struct btmrvl_sdio_card {
+        struct sdio_func *func;
+        u32 ioport;
+        const char *helper;
+        const char *firmware;
+        u8 rx_unit;
+        struct btmrvl_private *priv;
+};
+
+struct btmrvl_sdio_device {
+        const char *helper;
+        const char *firmware;
+};
+
+
+/* Platform specific DMA alignment */
+#define BTSDIO_DMA_ALIGN                8
+
+/* Macros for Data Alignment : size */
+#define ALIGN_SZ(p, a)  \
+        (((p) + ((a) - 1)) & ~((a) - 1))
+
+/* Macros for Data Alignment : address */
+#define ALIGN_ADDR(p, a)        \
+        ((((unsigned long)(p)) + (((unsigned long)(a)) - 1)) & \
+                                        ~(((unsigned long)(a)) - 1))
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index e70c57ee4221..7ba91aa3fe8b 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -35,7 +35,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 
-#define VERSION "0.5"
+#define VERSION "0.6"
 
 static int ignore_dga;
 static int ignore_csr;
@@ -145,6 +145,7 @@ static struct usb_device_id blacklist_table[] = {
 #define BTUSB_INTR_RUNNING      0
 #define BTUSB_BULK_RUNNING      1
 #define BTUSB_ISOC_RUNNING      2
+#define BTUSB_SUSPENDING        3
 
 struct btusb_data {
         struct hci_dev       *hdev;
@@ -157,11 +158,15 @@ struct btusb_data {
         unsigned long flags;
 
         struct work_struct work;
+        struct work_struct waker;
 
         struct usb_anchor tx_anchor;
         struct usb_anchor intr_anchor;
         struct usb_anchor bulk_anchor;
         struct usb_anchor isoc_anchor;
+        struct usb_anchor deferred;
+        int tx_in_flight;
+        spinlock_t txlock;
 
         struct usb_endpoint_descriptor *intr_ep;
         struct usb_endpoint_descriptor *bulk_tx_ep;
@@ -174,8 +179,23 @@ struct btusb_data {
         unsigned int sco_num;
         int isoc_altsetting;
         int suspend_count;
+        int did_iso_resume:1;
 };
 
+static int inc_tx(struct btusb_data *data)
+{
+        unsigned long flags;
+        int rv;
+
+        spin_lock_irqsave(&data->txlock, flags);
+        rv = test_bit(BTUSB_SUSPENDING, &data->flags);
+        if (!rv)
+                data->tx_in_flight++;
+        spin_unlock_irqrestore(&data->txlock, flags);
+
+        return rv;
+}
+
 static void btusb_intr_complete(struct urb *urb)
 {
         struct hci_dev *hdev = urb->context;
@@ -202,6 +222,7 @@ static void btusb_intr_complete(struct urb *urb)
         if (!test_bit(BTUSB_INTR_RUNNING, &data->flags))
                 return;
 
+        usb_mark_last_busy(data->udev);
         usb_anchor_urb(urb, &data->intr_anchor);
 
         err = usb_submit_urb(urb, GFP_ATOMIC);
@@ -301,7 +322,7 @@ static int btusb_submit_bulk_urb(struct hci_dev *hdev, gfp_t mem_flags)
         struct urb *urb;
         unsigned char *buf;
         unsigned int pipe;
-        int err, size;
+        int err, size = HCI_MAX_FRAME_SIZE;
 
         BT_DBG("%s", hdev->name);
 
@@ -312,8 +333,6 @@ static int btusb_submit_bulk_urb(struct hci_dev *hdev, gfp_t mem_flags)
         if (!urb)
                 return -ENOMEM;
 
-        size = le16_to_cpu(data->bulk_rx_ep->wMaxPacketSize);
-
         buf = kmalloc(size, mem_flags);
         if (!buf) {
                 usb_free_urb(urb);
@@ -327,6 +346,7 @@ static int btusb_submit_bulk_urb(struct hci_dev *hdev, gfp_t mem_flags)
 
         urb->transfer_flags |= URB_FREE_BUFFER;
 
+        usb_mark_last_busy(data->udev);
         usb_anchor_urb(urb, &data->bulk_anchor);
 
         err = usb_submit_urb(urb, mem_flags);
@@ -465,6 +485,33 @@ static void btusb_tx_complete(struct urb *urb)
 {
         struct sk_buff *skb = urb->context;
         struct hci_dev *hdev = (struct hci_dev *) skb->dev;
+        struct btusb_data *data = hdev->driver_data;
+
+        BT_DBG("%s urb %p status %d count %d", hdev->name,
+                                        urb, urb->status, urb->actual_length);
+
+        if (!test_bit(HCI_RUNNING, &hdev->flags))
+                goto done;
+
+        if (!urb->status)
+                hdev->stat.byte_tx += urb->transfer_buffer_length;
+        else
+                hdev->stat.err_tx++;
+
+done:
+        spin_lock(&data->txlock);
+        data->tx_in_flight--;
+        spin_unlock(&data->txlock);
+
+        kfree(urb->setup_packet);
+
+        kfree_skb(skb);
+}
+
+static void btusb_isoc_tx_complete(struct urb *urb)
+{
+        struct sk_buff *skb = urb->context;
+        struct hci_dev *hdev = (struct hci_dev *) skb->dev;
 
         BT_DBG("%s urb %p status %d count %d", hdev->name,
                                         urb, urb->status, urb->actual_length);
@@ -490,11 +537,17 @@ static int btusb_open(struct hci_dev *hdev)
 
         BT_DBG("%s", hdev->name);
 
+        err = usb_autopm_get_interface(data->intf);
+        if (err < 0)
+                return err;
+
+        data->intf->needs_remote_wakeup = 1;
+
         if (test_and_set_bit(HCI_RUNNING, &hdev->flags))
-                return 0;
+                goto done;
 
         if (test_and_set_bit(BTUSB_INTR_RUNNING, &data->flags))
-                return 0;
+                goto done;
 
         err = btusb_submit_intr_urb(hdev, GFP_KERNEL);
         if (err < 0)
@@ -509,17 +562,28 @@ static int btusb_open(struct hci_dev *hdev)
         set_bit(BTUSB_BULK_RUNNING, &data->flags);
         btusb_submit_bulk_urb(hdev, GFP_KERNEL);
 
+done:
+        usb_autopm_put_interface(data->intf);
         return 0;
 
 failed:
         clear_bit(BTUSB_INTR_RUNNING, &data->flags);
         clear_bit(HCI_RUNNING, &hdev->flags);
+        usb_autopm_put_interface(data->intf);
         return err;
 }
 
+static void btusb_stop_traffic(struct btusb_data *data)
+{
+        usb_kill_anchored_urbs(&data->intr_anchor);
+        usb_kill_anchored_urbs(&data->bulk_anchor);
+        usb_kill_anchored_urbs(&data->isoc_anchor);
+}
+
 static int btusb_close(struct hci_dev *hdev)
 {
         struct btusb_data *data = hdev->driver_data;
+        int err;
 
         BT_DBG("%s", hdev->name);
 
@@ -529,13 +593,16 @@ static int btusb_close(struct hci_dev *hdev)
         cancel_work_sync(&data->work);
 
         clear_bit(BTUSB_ISOC_RUNNING, &data->flags);
-        usb_kill_anchored_urbs(&data->isoc_anchor);
-
         clear_bit(BTUSB_BULK_RUNNING, &data->flags);
-        usb_kill_anchored_urbs(&data->bulk_anchor);
-
         clear_bit(BTUSB_INTR_RUNNING, &data->flags);
-        usb_kill_anchored_urbs(&data->intr_anchor);
+
+        btusb_stop_traffic(data);
+        err = usb_autopm_get_interface(data->intf);
+        if (err < 0)
+                return 0;
+
+        data->intf->needs_remote_wakeup = 0;
+        usb_autopm_put_interface(data->intf);
 
         return 0;
 }
@@ -622,7 +689,7 @@ static int btusb_send_frame(struct sk_buff *skb)
                 urb->dev      = data->udev;
                 urb->pipe     = pipe;
                 urb->context  = skb;
-                urb->complete = btusb_tx_complete;
+                urb->complete = btusb_isoc_tx_complete;
                 urb->interval = data->isoc_tx_ep->bInterval;
 
                 urb->transfer_flags  = URB_ISO_ASAP;
@@ -633,12 +700,21 @@ static int btusb_send_frame(struct sk_buff *skb)
                                 le16_to_cpu(data->isoc_tx_ep->wMaxPacketSize));
 
                 hdev->stat.sco_tx++;
-                break;
+                goto skip_waking;
 
         default:
                 return -EILSEQ;
         }
 
+        err = inc_tx(data);
+        if (err) {
+                usb_anchor_urb(urb, &data->deferred);
+                schedule_work(&data->waker);
+                err = 0;
+                goto done;
+        }
+
+skip_waking:
         usb_anchor_urb(urb, &data->tx_anchor);
 
         err = usb_submit_urb(urb, GFP_ATOMIC);
@@ -646,10 +722,13 @@ static int btusb_send_frame(struct sk_buff *skb)
                 BT_ERR("%s urb %p submission failed", hdev->name, urb);
                 kfree(urb->setup_packet);
                 usb_unanchor_urb(urb);
+        } else {
+                usb_mark_last_busy(data->udev);
         }
 
         usb_free_urb(urb);
 
+done:
         return err;
 }
 
@@ -721,8 +800,19 @@ static void btusb_work(struct work_struct *work)
 {
         struct btusb_data *data = container_of(work, struct btusb_data, work);
         struct hci_dev *hdev = data->hdev;
+        int err;
 
         if (hdev->conn_hash.sco_num > 0) {
+                if (!data->did_iso_resume) {
+                        err = usb_autopm_get_interface(data->isoc);
+                        if (err < 0) {
+                                clear_bit(BTUSB_ISOC_RUNNING, &data->flags);
+                                usb_kill_anchored_urbs(&data->isoc_anchor);
+                                return;
+                        }
+
+                        data->did_iso_resume = 1;
+                }
                 if (data->isoc_altsetting != 2) {
                         clear_bit(BTUSB_ISOC_RUNNING, &data->flags);
                         usb_kill_anchored_urbs(&data->isoc_anchor);
@@ -742,9 +832,25 @@ static void btusb_work(struct work_struct *work)
                 usb_kill_anchored_urbs(&data->isoc_anchor);
 
                 __set_isoc_interface(hdev, 0);
+                if (data->did_iso_resume) {
+                        data->did_iso_resume = 0;
+                        usb_autopm_put_interface(data->isoc);
+                }
         }
 }
 
+static void btusb_waker(struct work_struct *work)
+{
+        struct btusb_data *data = container_of(work, struct btusb_data, waker);
+        int err;
+
+        err = usb_autopm_get_interface(data->intf);
+        if (err < 0)
+                return;
+
+        usb_autopm_put_interface(data->intf);
+}
+
 static int btusb_probe(struct usb_interface *intf,
                                 const struct usb_device_id *id)
 {
@@ -814,11 +920,14 @@ static int btusb_probe(struct usb_interface *intf,
         spin_lock_init(&data->lock);
 
         INIT_WORK(&data->work, btusb_work);
+        INIT_WORK(&data->waker, btusb_waker);
+        spin_lock_init(&data->txlock);
 
         init_usb_anchor(&data->tx_anchor);
         init_usb_anchor(&data->intr_anchor);
         init_usb_anchor(&data->bulk_anchor);
         init_usb_anchor(&data->isoc_anchor);
+        init_usb_anchor(&data->deferred);
 
         hdev = hci_alloc_dev();
         if (!hdev) {
@@ -943,6 +1052,7 @@ static void btusb_disconnect(struct usb_interface *intf)
         hci_free_dev(hdev);
 }
 
+#ifdef CONFIG_PM
 static int btusb_suspend(struct usb_interface *intf, pm_message_t message)
 {
         struct btusb_data *data = usb_get_intfdata(intf);
@@ -952,22 +1062,44 @@ static int btusb_suspend(struct usb_interface *intf, pm_message_t message)
         if (data->suspend_count++)
                 return 0;
 
+        spin_lock_irq(&data->txlock);
+        if (!(interface_to_usbdev(intf)->auto_pm && data->tx_in_flight)) {
+                set_bit(BTUSB_SUSPENDING, &data->flags);
+                spin_unlock_irq(&data->txlock);
+        } else {
+                spin_unlock_irq(&data->txlock);
+                data->suspend_count--;
+                return -EBUSY;
+        }
+
         cancel_work_sync(&data->work);
 
+        btusb_stop_traffic(data);
         usb_kill_anchored_urbs(&data->tx_anchor);
 
-        usb_kill_anchored_urbs(&data->isoc_anchor);
-        usb_kill_anchored_urbs(&data->bulk_anchor);
-        usb_kill_anchored_urbs(&data->intr_anchor);
-
         return 0;
 }
 
+static void play_deferred(struct btusb_data *data)
+{
+        struct urb *urb;
+        int err;
+
+        while ((urb = usb_get_from_anchor(&data->deferred))) {
+                err = usb_submit_urb(urb, GFP_ATOMIC);
+                if (err < 0)
+                        break;
+
+                data->tx_in_flight++;
+        }
+        usb_scuttle_anchored_urbs(&data->deferred);
+}
+
 static int btusb_resume(struct usb_interface *intf)
 {
         struct btusb_data *data = usb_get_intfdata(intf);
         struct hci_dev *hdev = data->hdev;
-        int err;
+        int err = 0;
 
         BT_DBG("intf %p", intf);
 
@@ -975,13 +1107,13 @@ static int btusb_resume(struct usb_interface *intf)
                 return 0;
 
         if (!test_bit(HCI_RUNNING, &hdev->flags))
-                return 0;
+                goto done;
 
         if (test_bit(BTUSB_INTR_RUNNING, &data->flags)) {
                 err = btusb_submit_intr_urb(hdev, GFP_NOIO);
                 if (err < 0) {
                         clear_bit(BTUSB_INTR_RUNNING, &data->flags);
-                        return err;
+                        goto failed;
                 }
         }
 
@@ -989,9 +1121,10 @@ static int btusb_resume(struct usb_interface *intf)
                 err = btusb_submit_bulk_urb(hdev, GFP_NOIO);
                 if (err < 0) {
                         clear_bit(BTUSB_BULK_RUNNING, &data->flags);
-                        return err;
-                } else
-                        btusb_submit_bulk_urb(hdev, GFP_NOIO);
+                        goto failed;
+                }
+
+                btusb_submit_bulk_urb(hdev, GFP_NOIO);
         }
 
         if (test_bit(BTUSB_ISOC_RUNNING, &data->flags)) {
@@ -1001,16 +1134,35 @@ static int btusb_resume(struct usb_interface *intf)
                         btusb_submit_isoc_urb(hdev, GFP_NOIO);
         }
 
+        spin_lock_irq(&data->txlock);
+        play_deferred(data);
+        clear_bit(BTUSB_SUSPENDING, &data->flags);
+        spin_unlock_irq(&data->txlock);
+        schedule_work(&data->work);
+
         return 0;
+
+failed:
+        usb_scuttle_anchored_urbs(&data->deferred);
+done:
+        spin_lock_irq(&data->txlock);
+        clear_bit(BTUSB_SUSPENDING, &data->flags);
+        spin_unlock_irq(&data->txlock);
+
+        return err;
 }
+#endif
 
 static struct usb_driver btusb_driver = {
         .name           = "btusb",
         .probe          = btusb_probe,
         .disconnect     = btusb_disconnect,
+#ifdef CONFIG_PM
         .suspend        = btusb_suspend,
         .resume         = btusb_resume,
+#endif
         .id_table       = btusb_table,
+        .supports_autosuspend = 1,
 };
 
 static int __init btusb_init(void)
diff --git a/drivers/bluetooth/hci_bcsp.c b/drivers/bluetooth/hci_bcsp.c
index 894b2cb11ea6..40aec0fb8596 100644
--- a/drivers/bluetooth/hci_bcsp.c
+++ b/drivers/bluetooth/hci_bcsp.c
@@ -373,8 +373,9 @@ static void bcsp_pkt_cull(struct bcsp_struct *bcsp)
 
         i = 0;
         skb_queue_walk_safe(&bcsp->unack, skb, tmp) {
-                if (i++ >= pkts_to_be_removed)
+                if (i >= pkts_to_be_removed)
                         break;
+                i++;
 
                 __skb_unlink(skb, &bcsp->unack);
                 kfree_skb(skb);
diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index 968166a45f86..718394e2c01e 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -138,8 +138,11 @@ struct sock *bt_accept_dequeue(struct sock *parent, struct socket *newsock);
 struct bt_skb_cb {
         __u8 pkt_type;
         __u8 incoming;
+        __u8 tx_seq;
+        __u8 retries;
+        __u8 sar;
 };
-#define bt_cb(skb) ((struct bt_skb_cb *)(skb->cb)) 
+#define bt_cb(skb) ((struct bt_skb_cb *)((skb)->cb))
 
 static inline struct sk_buff *bt_skb_alloc(unsigned int len, gfp_t how)
 {
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index c4ca4228b083..7b640aeddb64 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -117,7 +117,7 @@ struct hci_dev {
         struct sk_buff          *sent_cmd;
         struct sk_buff          *reassembly[3];
 
-        struct semaphore        req_lock;
+        struct mutex            req_lock;
         wait_queue_head_t       req_wait_q;
         __u32                   req_status;
         __u32                   req_result;
@@ -187,6 +187,7 @@ struct hci_conn {
         struct work_struct work_del;
 
         struct device   dev;
+        atomic_t        devref;
 
         struct hci_dev  *hdev;
         void            *l2cap_data;
@@ -339,6 +340,9 @@ int hci_conn_switch_role(struct hci_conn *conn, __u8 role);
 void hci_conn_enter_active_mode(struct hci_conn *conn);
 void hci_conn_enter_sniff_mode(struct hci_conn *conn);
 
+void hci_conn_hold_device(struct hci_conn *conn);
+void hci_conn_put_device(struct hci_conn *conn);
+
 static inline void hci_conn_hold(struct hci_conn *conn)
 {
         atomic_inc(&conn->refcnt);
@@ -700,8 +704,8 @@ struct hci_sec_filter {
 #define HCI_REQ_PEND      1
 #define HCI_REQ_CANCELED  2
 
-#define hci_req_lock(d)         down(&d->req_lock)
-#define hci_req_unlock(d)       up(&d->req_lock)
+#define hci_req_lock(d)         mutex_lock(&d->req_lock)
+#define hci_req_unlock(d)       mutex_unlock(&d->req_lock)
 
 void hci_req_complete(struct hci_dev *hdev, int result);
 
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index e919fca1072a..9516f4b4a3c2 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -27,12 +27,14 @@
 
 /* L2CAP defaults */
 #define L2CAP_DEFAULT_MTU               672
+#define L2CAP_DEFAULT_MIN_MTU           48
 #define L2CAP_DEFAULT_FLUSH_TO          0xffff
-#define L2CAP_DEFAULT_RX_WINDOW         1
-#define L2CAP_DEFAULT_MAX_RECEIVE       1
-#define L2CAP_DEFAULT_RETRANS_TO        300    /* 300 milliseconds */
-#define L2CAP_DEFAULT_MONITOR_TO        1000   /* 1 second */
-#define L2CAP_DEFAULT_MAX_RX_APDU       0xfff7
+#define L2CAP_DEFAULT_TX_WINDOW         63
+#define L2CAP_DEFAULT_NUM_TO_ACK        (L2CAP_DEFAULT_TX_WINDOW/5)
+#define L2CAP_DEFAULT_MAX_TX            3
+#define L2CAP_DEFAULT_RETRANS_TO        1000    /* 1 second */
+#define L2CAP_DEFAULT_MONITOR_TO        12000   /* 12 seconds */
+#define L2CAP_DEFAULT_MAX_PDU_SIZE      672
 
 #define L2CAP_CONN_TIMEOUT      (40000) /* 40 seconds */
 #define L2CAP_INFO_TIMEOUT      (4000)  /*  4 seconds */
@@ -52,6 +54,7 @@ struct l2cap_options {
         __u16 imtu;
         __u16 flush_to;
         __u8  mode;
+        __u8  fcs;
 };
 
 #define L2CAP_CONNINFO  0x02
@@ -93,6 +96,32 @@ struct l2cap_conninfo {
 #define L2CAP_FCS_NONE          0x00
 #define L2CAP_FCS_CRC16         0x01
 
+/* L2CAP Control Field bit masks */
+#define L2CAP_CTRL_SAR               0xC000
+#define L2CAP_CTRL_REQSEQ            0x3F00
+#define L2CAP_CTRL_TXSEQ             0x007E
+#define L2CAP_CTRL_RETRANS           0x0080
+#define L2CAP_CTRL_FINAL             0x0080
+#define L2CAP_CTRL_POLL              0x0010
+#define L2CAP_CTRL_SUPERVISE         0x000C
+#define L2CAP_CTRL_FRAME_TYPE        0x0001 /* I- or S-Frame */
+
+#define L2CAP_CTRL_TXSEQ_SHIFT      1
+#define L2CAP_CTRL_REQSEQ_SHIFT     8
+#define L2CAP_CTRL_SAR_SHIFT       14
+
+/* L2CAP Supervisory Function */
+#define L2CAP_SUPER_RCV_READY           0x0000
+#define L2CAP_SUPER_REJECT              0x0004
+#define L2CAP_SUPER_RCV_NOT_READY       0x0008
+#define L2CAP_SUPER_SELECT_REJECT       0x000C
+
+/* L2CAP Segmentation and Reassembly */
+#define L2CAP_SDU_UNSEGMENTED       0x0000
+#define L2CAP_SDU_START             0x4000
+#define L2CAP_SDU_END               0x8000
+#define L2CAP_SDU_CONTINUE          0xC000
+
 /* L2CAP structures */
 struct l2cap_hdr {
         __le16     len;
@@ -190,7 +219,7 @@ struct l2cap_conf_rfc {
 #define L2CAP_MODE_RETRANS      0x01
 #define L2CAP_MODE_FLOWCTL      0x02
 #define L2CAP_MODE_ERTM         0x03
-#define L2CAP_MODE_STREAM       0x04
+#define L2CAP_MODE_STREAMING    0x04
 
 struct l2cap_disconn_req {
         __le16     dcid;
@@ -261,6 +290,14 @@ struct l2cap_conn {
 
 /* ----- L2CAP channel and socket info ----- */
 #define l2cap_pi(sk) ((struct l2cap_pinfo *) sk)
+#define TX_QUEUE(sk) (&l2cap_pi(sk)->tx_queue)
+#define SREJ_QUEUE(sk) (&l2cap_pi(sk)->srej_queue)
+#define SREJ_LIST(sk) (&l2cap_pi(sk)->srej_l.list)
+
+struct srej_list {
+        __u8    tx_seq;
+        struct list_head list;
+};
 
 struct l2cap_pinfo {
         struct bt_sock  bt;
@@ -271,30 +308,97 @@ struct l2cap_pinfo {
         __u16           imtu;
         __u16           omtu;
         __u16           flush_to;
-        __u8            sec_level;
+        __u8            mode;
+        __u8            num_conf_req;
+        __u8            num_conf_rsp;
+
+        __u8            fcs;
+        __u8            sec_level;
         __u8            role_switch;
-        __u8            force_reliable;
+        __u8            force_reliable;
 
         __u8            conf_req[64];
         __u8            conf_len;
         __u8            conf_state;
-        __u8            conf_retry;
+        __u8            conn_state;
+
+        __u8            next_tx_seq;
+        __u8            expected_ack_seq;
+        __u8            req_seq;
+        __u8            expected_tx_seq;
+        __u8            buffer_seq;
+        __u8            buffer_seq_srej;
+        __u8            srej_save_reqseq;
+        __u8            unacked_frames;
+        __u8            retry_count;
+        __u8            num_to_ack;
+        __u16           sdu_len;
+        __u16           partial_sdu_len;
+        struct sk_buff  *sdu;
 
         __u8            ident;
 
+        __u8            remote_tx_win;
+        __u8            remote_max_tx;
+        __u16           retrans_timeout;
+        __u16           monitor_timeout;
+        __u16           max_pdu_size;
+
         __le16          sport;
 
+        struct timer_list       retrans_timer;
+        struct timer_list       monitor_timer;
+        struct sk_buff_head     tx_queue;
+        struct sk_buff_head     srej_queue;
+        struct srej_list        srej_l;
         struct l2cap_conn       *conn;
         struct sock             *next_c;
         struct sock             *prev_c;
 };
 
-#define L2CAP_CONF_REQ_SENT     0x01
-#define L2CAP_CONF_INPUT_DONE   0x02
-#define L2CAP_CONF_OUTPUT_DONE  0x04
-#define L2CAP_CONF_CONNECT_PEND 0x80
-
-#define L2CAP_CONF_MAX_RETRIES  2
+#define L2CAP_CONF_REQ_SENT       0x01
+#define L2CAP_CONF_INPUT_DONE     0x02
+#define L2CAP_CONF_OUTPUT_DONE    0x04
+#define L2CAP_CONF_MTU_DONE       0x08
+#define L2CAP_CONF_MODE_DONE      0x10
+#define L2CAP_CONF_CONNECT_PEND   0x20
+#define L2CAP_CONF_NO_FCS_RECV    0x40
+#define L2CAP_CONF_STATE2_DEVICE  0x80
+
+#define L2CAP_CONF_MAX_CONF_REQ 2
+#define L2CAP_CONF_MAX_CONF_RSP 2
+
+#define L2CAP_CONN_SAR_SDU         0x01
+#define L2CAP_CONN_SREJ_SENT       0x02
+#define L2CAP_CONN_WAIT_F          0x04
+#define L2CAP_CONN_SREJ_ACT        0x08
+#define L2CAP_CONN_SEND_PBIT       0x10
+#define L2CAP_CONN_REMOTE_BUSY     0x20
+#define L2CAP_CONN_LOCAL_BUSY      0x40
+
+#define __mod_retrans_timer() mod_timer(&l2cap_pi(sk)->retrans_timer, \
+                jiffies +  msecs_to_jiffies(L2CAP_DEFAULT_RETRANS_TO));
+#define __mod_monitor_timer() mod_timer(&l2cap_pi(sk)->monitor_timer, \
+                jiffies + msecs_to_jiffies(L2CAP_DEFAULT_MONITOR_TO));
+
+static inline int l2cap_tx_window_full(struct sock *sk)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        int sub;
+
+        sub = (pi->next_tx_seq - pi->expected_ack_seq) % 64;
+
+        if (sub < 0)
+                sub += 64;
+
+        return (sub == pi->remote_tx_win);
+}
+
+#define __get_txseq(ctrl) ((ctrl) & L2CAP_CTRL_TXSEQ) >> 1
+#define __get_reqseq(ctrl) ((ctrl) & L2CAP_CTRL_REQSEQ) >> 8
+#define __is_iframe(ctrl) !((ctrl) & L2CAP_CTRL_FRAME_TYPE)
+#define __is_sframe(ctrl) (ctrl) & L2CAP_CTRL_FRAME_TYPE
+#define __is_sar_start(ctrl) ((ctrl) & L2CAP_CTRL_SAR) == L2CAP_SDU_START
 
 void l2cap_load(void);
 
diff --git a/include/net/bluetooth/rfcomm.h b/include/net/bluetooth/rfcomm.h
index c274993234e3..921d7b3c7f8d 100644
--- a/include/net/bluetooth/rfcomm.h
+++ b/include/net/bluetooth/rfcomm.h
@@ -29,6 +29,7 @@
 #define RFCOMM_CONN_TIMEOUT (HZ * 30)
 #define RFCOMM_DISC_TIMEOUT (HZ * 20)
 #define RFCOMM_AUTH_TIMEOUT (HZ * 25)
+#define RFCOMM_IDLE_TIMEOUT (HZ * 2)
 
 #define RFCOMM_DEFAULT_MTU      127
 #define RFCOMM_DEFAULT_CREDITS  7
@@ -154,6 +155,7 @@ struct rfcomm_msc {
 struct rfcomm_session {
         struct list_head list;
         struct socket   *sock;
+        struct timer_list timer;
         unsigned long    state;
         unsigned long    flags;
         atomic_t         refcnt;
diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 59fdb1d2e8ed..ed371684c133 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -34,6 +34,7 @@ menuconfig BT
 config BT_L2CAP
         tristate "L2CAP protocol support"
         depends on BT
+        select CRC16
         help
           L2CAP (Logical Link Control and Adaptation Protocol) provides
           connection oriented and connection-less data transport.  L2CAP
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index fa47d5d84f5c..a9750984f772 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -246,6 +246,8 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
         if (hdev->notify)
                 hdev->notify(hdev, HCI_NOTIFY_CONN_ADD);
 
+        atomic_set(&conn->devref, 0);
+
         hci_conn_init_sysfs(conn);
 
         tasklet_enable(&hdev->tx_task);
@@ -288,7 +290,7 @@ int hci_conn_del(struct hci_conn *conn)
 
         skb_queue_purge(&conn->data_q);
 
-        hci_conn_del_sysfs(conn);
+        hci_conn_put_device(conn);
 
         hci_dev_put(hdev);
 
@@ -583,6 +585,19 @@ void hci_conn_check_pending(struct hci_dev *hdev)
         hci_dev_unlock(hdev);
 }
 
+void hci_conn_hold_device(struct hci_conn *conn)
+{
+        atomic_inc(&conn->devref);
+}
+EXPORT_SYMBOL(hci_conn_hold_device);
+
+void hci_conn_put_device(struct hci_conn *conn)
+{
+        if (atomic_dec_and_test(&conn->devref))
+                hci_conn_del_sysfs(conn);
+}
+EXPORT_SYMBOL(hci_conn_put_device);
+
 int hci_get_conn_list(void __user *arg)
 {
         struct hci_conn_list_req req, *cl;
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 406ad07cdea1..e1da8f68759c 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -911,7 +911,7 @@ int hci_register_dev(struct hci_dev *hdev)
                 hdev->reassembly[i] = NULL;
 
         init_waitqueue_head(&hdev->req_wait_q);
-        init_MUTEX(&hdev->req_lock);
+        mutex_init(&hdev->req_lock);
 
         inquiry_cache_init(hdev);
 
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 184ba0a88ec0..e99fe385fba2 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -887,6 +887,7 @@ static inline void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *s
                 } else
                         conn->state = BT_CONNECTED;
 
+                hci_conn_hold_device(conn);
                 hci_conn_add_sysfs(conn);
 
                 if (test_bit(HCI_AUTH, &hdev->flags))
@@ -1693,6 +1694,7 @@ static inline void hci_sync_conn_complete_evt(struct hci_dev *hdev, struct sk_bu
                 conn->handle = __le16_to_cpu(ev->handle);
                 conn->state  = BT_CONNECTED;
 
+                hci_conn_hold_device(conn);
                 hci_conn_add_sysfs(conn);
                 break;
 
diff --git a/net/bluetooth/hidp/core.c b/net/bluetooth/hidp/core.c
index b18676870d55..09bedeb5579c 100644
--- a/net/bluetooth/hidp/core.c
+++ b/net/bluetooth/hidp/core.c
@@ -40,6 +40,7 @@
 
 #include <linux/input.h>
 #include <linux/hid.h>
+#include <linux/hidraw.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
@@ -92,10 +93,14 @@ static void __hidp_link_session(struct hidp_session *session)
 {
         __module_get(THIS_MODULE);
         list_add(&session->list, &hidp_session_list);
+
+        hci_conn_hold_device(session->conn);
 }
 
 static void __hidp_unlink_session(struct hidp_session *session)
 {
+        hci_conn_put_device(session->conn);
+
         list_del(&session->list);
         module_put(THIS_MODULE);
 }
@@ -374,6 +379,7 @@ static void hidp_process_hid_control(struct hidp_session *session,
 
                 /* Kill session thread */
                 atomic_inc(&session->terminate);
+                hidp_schedule(session);
         }
 }
 
@@ -573,7 +579,11 @@ static int hidp_session(void *arg)
         if (session->hid) {
                 if (session->hid->claimed & HID_CLAIMED_INPUT)
                         hidinput_disconnect(session->hid);
+                if (session->hid->claimed & HID_CLAIMED_HIDRAW)
+                        hidraw_disconnect(session->hid);
+
                 hid_destroy_device(session->hid);
+                session->hid = NULL;
         }
 
         /* Wakeup user-space polling for socket errors */
@@ -601,25 +611,27 @@ static struct device *hidp_get_device(struct hidp_session *session)
 {
         bdaddr_t *src = &bt_sk(session->ctrl_sock->sk)->src;
         bdaddr_t *dst = &bt_sk(session->ctrl_sock->sk)->dst;
+        struct device *device = NULL;
         struct hci_dev *hdev;
-        struct hci_conn *conn;
 
         hdev = hci_get_route(dst, src);
         if (!hdev)
                 return NULL;
 
-        conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst);
+        session->conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst);
+        if (session->conn)
+                device = &session->conn->dev;
 
         hci_dev_put(hdev);
 
-        return conn ? &conn->dev : NULL;
+        return device;
 }
 
 static int hidp_setup_input(struct hidp_session *session,
                                 struct hidp_connadd_req *req)
 {
         struct input_dev *input;
-        int i;
+        int err, i;
 
         input = input_allocate_device();
         if (!input)
@@ -666,7 +678,13 @@ static int hidp_setup_input(struct hidp_session *session,
 
         input->event = hidp_input_event;
 
-        return input_register_device(input);
+        err = input_register_device(input);
+        if (err < 0) {
+                hci_conn_put_device(session->conn);
+                return err;
+        }
+
+        return 0;
 }
 
 static int hidp_open(struct hid_device *hid)
@@ -748,13 +766,11 @@ static int hidp_setup_hid(struct hidp_session *session,
 {
         struct hid_device *hid;
         bdaddr_t src, dst;
-        int ret;
+        int err;
 
         hid = hid_allocate_device();
-        if (IS_ERR(hid)) {
-                ret = PTR_ERR(session->hid);
-                goto err;
-        }
+        if (IS_ERR(hid))
+                return PTR_ERR(session->hid);
 
         session->hid = hid;
         session->req = req;
@@ -776,16 +792,17 @@ static int hidp_setup_hid(struct hidp_session *session,
         hid->dev.parent = hidp_get_device(session);
         hid->ll_driver = &hidp_hid_driver;
 
-        ret = hid_add_device(hid);
-        if (ret)
-                goto err_hid;
+        err = hid_add_device(hid);
+        if (err < 0)
+                goto failed;
 
         return 0;
-err_hid:
+
+failed:
         hid_destroy_device(hid);
         session->hid = NULL;
-err:
-        return ret;
+
+        return err;
 }
 
 int hidp_add_connection(struct hidp_connadd_req *req, struct socket *ctrl_sock, struct socket *intr_sock)
@@ -835,13 +852,13 @@ int hidp_add_connection(struct hidp_connadd_req *req, struct socket *ctrl_sock,
         if (req->rd_size > 0) {
                 err = hidp_setup_hid(session, req);
                 if (err && err != -ENODEV)
-                        goto err_skb;
+                        goto purge;
         }
 
         if (!session->hid) {
                 err = hidp_setup_input(session, req);
                 if (err < 0)
-                        goto err_skb;
+                        goto purge;
         }
 
         __hidp_link_session(session);
@@ -869,13 +886,20 @@ unlink:
 
         __hidp_unlink_session(session);
 
-        if (session->input)
+        if (session->input) {
                 input_unregister_device(session->input);
-        if (session->hid)
+                session->input = NULL;
+        }
+
+        if (session->hid) {
                 hid_destroy_device(session->hid);
-err_skb:
+                session->hid = NULL;
+        }
+
+purge:
         skb_queue_purge(&session->ctrl_transmit);
         skb_queue_purge(&session->intr_transmit);
+
 failed:
         up_write(&hidp_session_sem);
 
diff --git a/net/bluetooth/hidp/hidp.h b/net/bluetooth/hidp/hidp.h
index e503c89057ad..faf3d74c3586 100644
--- a/net/bluetooth/hidp/hidp.h
+++ b/net/bluetooth/hidp/hidp.h
@@ -126,6 +126,8 @@ int hidp_get_conninfo(struct hidp_conninfo *ci);
 struct hidp_session {
         struct list_head list;
 
+        struct hci_conn *conn;
+
         struct socket *ctrl_sock;
         struct socket *intr_sock;
 
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index bd0a4c1bced0..b03012564647 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -41,6 +41,7 @@
 #include <linux/list.h>
 #include <linux/device.h>
 #include <linux/uaccess.h>
+#include <linux/crc16.h>
 #include <net/sock.h>
 
 #include <asm/system.h>
@@ -50,7 +51,9 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 
-#define VERSION "2.13"
+#define VERSION "2.14"
+
+static int enable_ertm = 0;
 
 static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN;
 static u8 l2cap_fixed_chan[8] = { 0x02, };
@@ -331,6 +334,48 @@ static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16
         return hci_send_acl(conn->hcon, skb, 0);
 }
 
+static inline int l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
+{
+        struct sk_buff *skb;
+        struct l2cap_hdr *lh;
+        struct l2cap_conn *conn = pi->conn;
+        int count, hlen = L2CAP_HDR_SIZE + 2;
+
+        if (pi->fcs == L2CAP_FCS_CRC16)
+                hlen += 2;
+
+        BT_DBG("pi %p, control 0x%2.2x", pi, control);
+
+        count = min_t(unsigned int, conn->mtu, hlen);
+        control |= L2CAP_CTRL_FRAME_TYPE;
+
+        skb = bt_skb_alloc(count, GFP_ATOMIC);
+        if (!skb)
+                return -ENOMEM;
+
+        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
+        lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
+        lh->cid = cpu_to_le16(pi->dcid);
+        put_unaligned_le16(control, skb_put(skb, 2));
+
+        if (pi->fcs == L2CAP_FCS_CRC16) {
+                u16 fcs = crc16(0, (u8 *)lh, count - 2);
+                put_unaligned_le16(fcs, skb_put(skb, 2));
+        }
+
+        return hci_send_acl(pi->conn->hcon, skb, 0);
+}
+
+static inline int l2cap_send_rr_or_rnr(struct l2cap_pinfo *pi, u16 control)
+{
+        if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY)
+                control |= L2CAP_SUPER_RCV_NOT_READY;
+        else
+                control |= L2CAP_SUPER_RCV_READY;
+
+        return l2cap_send_sframe(pi, control);
+}
+
 static void l2cap_do_start(struct sock *sk)
 {
         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
@@ -364,6 +409,16 @@ static void l2cap_do_start(struct sock *sk)
         }
 }
 
+static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk)
+{
+        struct l2cap_disconn_req req;
+
+        req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
+        req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
+        l2cap_send_cmd(conn, l2cap_get_ident(conn),
+                        L2CAP_DISCONN_REQ, sizeof(req), &req);
+}
+
 /* ---- L2CAP connections ---- */
 static void l2cap_conn_start(struct l2cap_conn *conn)
 {
@@ -648,15 +703,10 @@ static void __l2cap_sock_close(struct sock *sk, int reason)
         case BT_CONFIG:
                 if (sk->sk_type == SOCK_SEQPACKET) {
                         struct l2cap_conn *conn = l2cap_pi(sk)->conn;
-                        struct l2cap_disconn_req req;
 
                         sk->sk_state = BT_DISCONN;
                         l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
-
-                        req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
-                        req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
-                        l2cap_send_cmd(conn, l2cap_get_ident(conn),
-                                        L2CAP_DISCONN_REQ, sizeof(req), &req);
+                        l2cap_send_disconn_req(conn, sk);
                 } else
                         l2cap_chan_del(sk, reason);
                 break;
@@ -715,12 +765,16 @@ static void l2cap_sock_init(struct sock *sk, struct sock *parent)
 
                 pi->imtu = l2cap_pi(parent)->imtu;
                 pi->omtu = l2cap_pi(parent)->omtu;
+                pi->mode = l2cap_pi(parent)->mode;
+                pi->fcs  = l2cap_pi(parent)->fcs;
                 pi->sec_level = l2cap_pi(parent)->sec_level;
                 pi->role_switch = l2cap_pi(parent)->role_switch;
                 pi->force_reliable = l2cap_pi(parent)->force_reliable;
         } else {
                 pi->imtu = L2CAP_DEFAULT_MTU;
                 pi->omtu = 0;
+                pi->mode = L2CAP_MODE_BASIC;
+                pi->fcs  = L2CAP_FCS_CRC16;
                 pi->sec_level = BT_SECURITY_LOW;
                 pi->role_switch = 0;
                 pi->force_reliable = 0;
@@ -956,6 +1010,19 @@ static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, int al
                 goto done;
         }
 
+        switch (l2cap_pi(sk)->mode) {
+        case L2CAP_MODE_BASIC:
+                break;
+        case L2CAP_MODE_ERTM:
+        case L2CAP_MODE_STREAMING:
+                if (enable_ertm)
+                        break;
+                /* fall through */
+        default:
+                err = -ENOTSUPP;
+                goto done;
+        }
+
         switch (sk->sk_state) {
         case BT_CONNECT:
         case BT_CONNECT2:
@@ -1007,6 +1074,19 @@ static int l2cap_sock_listen(struct socket *sock, int backlog)
                 goto done;
         }
 
+        switch (l2cap_pi(sk)->mode) {
+        case L2CAP_MODE_BASIC:
+                break;
+        case L2CAP_MODE_ERTM:
+        case L2CAP_MODE_STREAMING:
+                if (enable_ertm)
+                        break;
+                /* fall through */
+        default:
+                err = -ENOTSUPP;
+                goto done;
+        }
+
         if (!l2cap_pi(sk)->psm) {
                 bdaddr_t *src = &bt_sk(sk)->src;
                 u16 psm;
@@ -1117,39 +1197,219 @@ static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *l
         return 0;
 }
 
-static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len)
+static void l2cap_monitor_timeout(unsigned long arg)
 {
-        struct l2cap_conn *conn = l2cap_pi(sk)->conn;
-        struct sk_buff *skb, **frag;
-        int err, hlen, count, sent = 0;
-        struct l2cap_hdr *lh;
+        struct sock *sk = (void *) arg;
+        u16 control;
 
-        BT_DBG("sk %p len %d", sk, len);
+        bh_lock_sock(sk);
+        if (l2cap_pi(sk)->retry_count >= l2cap_pi(sk)->remote_max_tx) {
+                l2cap_send_disconn_req(l2cap_pi(sk)->conn, sk);
+                return;
+        }
 
-        /* First fragment (with L2CAP header) */
-        if (sk->sk_type == SOCK_DGRAM)
-                hlen = L2CAP_HDR_SIZE + 2;
-        else
-                hlen = L2CAP_HDR_SIZE;
+        l2cap_pi(sk)->retry_count++;
+        __mod_monitor_timer();
 
-        count = min_t(unsigned int, (conn->mtu - hlen), len);
+        control = L2CAP_CTRL_POLL;
+        l2cap_send_rr_or_rnr(l2cap_pi(sk), control);
+        bh_unlock_sock(sk);
+}
 
-        skb = bt_skb_send_alloc(sk, hlen + count,
-                        msg->msg_flags & MSG_DONTWAIT, &err);
-        if (!skb)
-                return err;
+static void l2cap_retrans_timeout(unsigned long arg)
+{
+        struct sock *sk = (void *) arg;
+        u16 control;
 
-        /* Create L2CAP header */
-        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
-        lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
-        lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+        bh_lock_sock(sk);
+        l2cap_pi(sk)->retry_count = 1;
+        __mod_monitor_timer();
+
+        l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
+
+        control = L2CAP_CTRL_POLL;
+        l2cap_send_rr_or_rnr(l2cap_pi(sk), control);
+        bh_unlock_sock(sk);
+}
+
+static void l2cap_drop_acked_frames(struct sock *sk)
+{
+        struct sk_buff *skb;
+
+        while ((skb = skb_peek(TX_QUEUE(sk)))) {
+                if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
+                        break;
+
+                skb = skb_dequeue(TX_QUEUE(sk));
+                kfree_skb(skb);
+
+                l2cap_pi(sk)->unacked_frames--;
+        }
+
+        if (!l2cap_pi(sk)->unacked_frames)
+                del_timer(&l2cap_pi(sk)->retrans_timer);
 
-        if (sk->sk_type == SOCK_DGRAM)
-                put_unaligned(l2cap_pi(sk)->psm, (__le16 *) skb_put(skb, 2));
+        return;
+}
+
+static inline int l2cap_do_send(struct sock *sk, struct sk_buff *skb)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        int err;
+
+        BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len);
+
+        err = hci_send_acl(pi->conn->hcon, skb, 0);
+        if (err < 0)
+                kfree_skb(skb);
+
+        return err;
+}
+
+static int l2cap_streaming_send(struct sock *sk)
+{
+        struct sk_buff *skb, *tx_skb;
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        u16 control, fcs;
+        int err;
+
+        while ((skb = sk->sk_send_head)) {
+                tx_skb = skb_clone(skb, GFP_ATOMIC);
+
+                control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
+                control |= pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT;
+                put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
+
+                if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) {
+                        fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
+                        put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
+                }
+
+                err = l2cap_do_send(sk, tx_skb);
+                if (err < 0) {
+                        l2cap_send_disconn_req(pi->conn, sk);
+                        return err;
+                }
+
+                pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
+
+                if (skb_queue_is_last(TX_QUEUE(sk), skb))
+                        sk->sk_send_head = NULL;
+                else
+                        sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
+
+                skb = skb_dequeue(TX_QUEUE(sk));
+                kfree_skb(skb);
+        }
+        return 0;
+}
+
+static int l2cap_retransmit_frame(struct sock *sk, u8 tx_seq)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        struct sk_buff *skb, *tx_skb;
+        u16 control, fcs;
+        int err;
+
+        skb = skb_peek(TX_QUEUE(sk));
+        do {
+                if (bt_cb(skb)->tx_seq != tx_seq) {
+                        if (skb_queue_is_last(TX_QUEUE(sk), skb))
+                                break;
+                        skb = skb_queue_next(TX_QUEUE(sk), skb);
+                        continue;
+                }
+
+                if (pi->remote_max_tx &&
+                                bt_cb(skb)->retries == pi->remote_max_tx) {
+                        l2cap_send_disconn_req(pi->conn, sk);
+                        break;
+                }
+
+                tx_skb = skb_clone(skb, GFP_ATOMIC);
+                bt_cb(skb)->retries++;
+                control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
+                control |= (pi->req_seq << L2CAP_CTRL_REQSEQ_SHIFT)
+                                | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
+                put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
+
+                if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) {
+                        fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
+                        put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
+                }
+
+                err = l2cap_do_send(sk, tx_skb);
+                if (err < 0) {
+                        l2cap_send_disconn_req(pi->conn, sk);
+                        return err;
+                }
+                break;
+        } while(1);
+        return 0;
+}
+
+static int l2cap_ertm_send(struct sock *sk)
+{
+        struct sk_buff *skb, *tx_skb;
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        u16 control, fcs;
+        int err;
+
+        if (pi->conn_state & L2CAP_CONN_WAIT_F)
+                return 0;
+
+        while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))
+                        && !(pi->conn_state & L2CAP_CONN_REMOTE_BUSY)) {
+                tx_skb = skb_clone(skb, GFP_ATOMIC);
+
+                if (pi->remote_max_tx &&
+                                bt_cb(skb)->retries == pi->remote_max_tx) {
+                        l2cap_send_disconn_req(pi->conn, sk);
+                        break;
+                }
+
+                bt_cb(skb)->retries++;
+
+                control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
+                control |= (pi->req_seq << L2CAP_CTRL_REQSEQ_SHIFT)
+                                | (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
+                put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
+
+
+                if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) {
+                        fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2);
+                        put_unaligned_le16(fcs, skb->data + tx_skb->len - 2);
+                }
+
+                err = l2cap_do_send(sk, tx_skb);
+                if (err < 0) {
+                        l2cap_send_disconn_req(pi->conn, sk);
+                        return err;
+                }
+                __mod_retrans_timer();
+
+                bt_cb(skb)->tx_seq = pi->next_tx_seq;
+                pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
+
+                pi->unacked_frames++;
+
+                if (skb_queue_is_last(TX_QUEUE(sk), skb))
+                        sk->sk_send_head = NULL;
+                else
+                        sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb);
+        }
+
+        return 0;
+}
+
+static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
+{
+        struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+        struct sk_buff **frag;
+        int err, sent = 0;
 
         if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
-                err = -EFAULT;
-                goto fail;
+                return -EFAULT;
         }
 
         sent += count;
@@ -1162,33 +1422,173 @@ static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len)
 
                 *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err);
                 if (!*frag)
-                        goto fail;
-
-                if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) {
-                        err = -EFAULT;
-                        goto fail;
-                }
+                        return -EFAULT;
+                if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count))
+                        return -EFAULT;
 
                 sent += count;
                 len  -= count;
 
                 frag = &(*frag)->next;
         }
-        err = hci_send_acl(conn->hcon, skb, 0);
-        if (err < 0)
-                goto fail;
 
         return sent;
+}
 
-fail:
-        kfree_skb(skb);
-        return err;
+static struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
+{
+        struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+        struct sk_buff *skb;
+        int err, count, hlen = L2CAP_HDR_SIZE + 2;
+        struct l2cap_hdr *lh;
+
+        BT_DBG("sk %p len %d", sk, (int)len);
+
+        count = min_t(unsigned int, (conn->mtu - hlen), len);
+        skb = bt_skb_send_alloc(sk, count + hlen,
+                        msg->msg_flags & MSG_DONTWAIT, &err);
+        if (!skb)
+                return ERR_PTR(-ENOMEM);
+
+        /* Create L2CAP header */
+        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
+        lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
+        lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+        put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
+
+        err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
+        if (unlikely(err < 0)) {
+                kfree_skb(skb);
+                return ERR_PTR(err);
+        }
+        return skb;
+}
+
+static struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
+{
+        struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+        struct sk_buff *skb;
+        int err, count, hlen = L2CAP_HDR_SIZE;
+        struct l2cap_hdr *lh;
+
+        BT_DBG("sk %p len %d", sk, (int)len);
+
+        count = min_t(unsigned int, (conn->mtu - hlen), len);
+        skb = bt_skb_send_alloc(sk, count + hlen,
+                        msg->msg_flags & MSG_DONTWAIT, &err);
+        if (!skb)
+                return ERR_PTR(-ENOMEM);
+
+        /* Create L2CAP header */
+        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
+        lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
+        lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+
+        err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
+        if (unlikely(err < 0)) {
+                kfree_skb(skb);
+                return ERR_PTR(err);
+        }
+        return skb;
+}
+
+static struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control, u16 sdulen)
+{
+        struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+        struct sk_buff *skb;
+        int err, count, hlen = L2CAP_HDR_SIZE + 2;
+        struct l2cap_hdr *lh;
+
+        BT_DBG("sk %p len %d", sk, (int)len);
+
+        if (sdulen)
+                hlen += 2;
+
+        if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
+                hlen += 2;
+
+        count = min_t(unsigned int, (conn->mtu - hlen), len);
+        skb = bt_skb_send_alloc(sk, count + hlen,
+                        msg->msg_flags & MSG_DONTWAIT, &err);
+        if (!skb)
+                return ERR_PTR(-ENOMEM);
+
+        /* Create L2CAP header */
+        lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
+        lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
+        lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
+        put_unaligned_le16(control, skb_put(skb, 2));
+        if (sdulen)
+                put_unaligned_le16(sdulen, skb_put(skb, 2));
+
+        err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
+        if (unlikely(err < 0)) {
+                kfree_skb(skb);
+                return ERR_PTR(err);
+        }
+
+        if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16)
+                put_unaligned_le16(0, skb_put(skb, 2));
+
+        bt_cb(skb)->retries = 0;
+        return skb;
+}
+
+static inline int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        struct sk_buff *skb;
+        struct sk_buff_head sar_queue;
+        u16 control;
+        size_t size = 0;
+
+        __skb_queue_head_init(&sar_queue);
+        control = L2CAP_SDU_START;
+        skb = l2cap_create_iframe_pdu(sk, msg, pi->max_pdu_size, control, len);
+        if (IS_ERR(skb))
+                return PTR_ERR(skb);
+
+        __skb_queue_tail(&sar_queue, skb);
+        len -= pi->max_pdu_size;
+        size +=pi->max_pdu_size;
+        control = 0;
+
+        while (len > 0) {
+                size_t buflen;
+
+                if (len > pi->max_pdu_size) {
+                        control |= L2CAP_SDU_CONTINUE;
+                        buflen = pi->max_pdu_size;
+                } else {
+                        control |= L2CAP_SDU_END;
+                        buflen = len;
+                }
+
+                skb = l2cap_create_iframe_pdu(sk, msg, buflen, control, 0);
+                if (IS_ERR(skb)) {
+                        skb_queue_purge(&sar_queue);
+                        return PTR_ERR(skb);
+                }
+
+                __skb_queue_tail(&sar_queue, skb);
+                len -= buflen;
+                size += buflen;
+                control = 0;
+        }
+        skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
+        if (sk->sk_send_head == NULL)
+                sk->sk_send_head = sar_queue.next;
+
+        return size;
 }
 
 static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
 {
         struct sock *sk = sock->sk;
-        int err = 0;
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        struct sk_buff *skb;
+        u16 control;
+        int err;
 
         BT_DBG("sock %p, sk %p", sock, sk);
 
@@ -1200,16 +1600,73 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
                 return -EOPNOTSUPP;
 
         /* Check outgoing MTU */
-        if (sk->sk_type != SOCK_RAW && len > l2cap_pi(sk)->omtu)
+        if (sk->sk_type == SOCK_SEQPACKET && pi->mode == L2CAP_MODE_BASIC
+                        && len > pi->omtu)
                 return -EINVAL;
 
         lock_sock(sk);
 
-        if (sk->sk_state == BT_CONNECTED)
-                err = l2cap_do_send(sk, msg, len);
-        else
+        if (sk->sk_state != BT_CONNECTED) {
                 err = -ENOTCONN;
+                goto done;
+        }
+
+        /* Connectionless channel */
+        if (sk->sk_type == SOCK_DGRAM) {
+                skb = l2cap_create_connless_pdu(sk, msg, len);
+                err = l2cap_do_send(sk, skb);
+                goto done;
+        }
+
+        switch (pi->mode) {
+        case L2CAP_MODE_BASIC:
+                /* Create a basic PDU */
+                skb = l2cap_create_basic_pdu(sk, msg, len);
+                if (IS_ERR(skb)) {
+                        err = PTR_ERR(skb);
+                        goto done;
+                }
+
+                err = l2cap_do_send(sk, skb);
+                if (!err)
+                        err = len;
+                break;
+
+        case L2CAP_MODE_ERTM:
+        case L2CAP_MODE_STREAMING:
+                /* Entire SDU fits into one PDU */
+                if (len <= pi->max_pdu_size) {
+                        control = L2CAP_SDU_UNSEGMENTED;
+                        skb = l2cap_create_iframe_pdu(sk, msg, len, control, 0);
+                        if (IS_ERR(skb)) {
+                                err = PTR_ERR(skb);
+                                goto done;
+                        }
+                        __skb_queue_tail(TX_QUEUE(sk), skb);
+                        if (sk->sk_send_head == NULL)
+                                sk->sk_send_head = skb;
+                } else {
+                /* Segment SDU into multiples PDUs */
+                        err = l2cap_sar_segment_sdu(sk, msg, len);
+                        if (err < 0)
+                                goto done;
+                }
+
+                if (pi->mode == L2CAP_MODE_STREAMING)
+                        err = l2cap_streaming_send(sk);
+                else
+                        err = l2cap_ertm_send(sk);
+
+                if (!err)
+                        err = len;
+                break;
+
+        default:
+                BT_DBG("bad state %1.1x", pi->mode);
+                err = -EINVAL;
+        }
 
+done:
         release_sock(sk);
         return err;
 }
@@ -1257,7 +1714,8 @@ static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __us
                 opts.imtu     = l2cap_pi(sk)->imtu;
                 opts.omtu     = l2cap_pi(sk)->omtu;
                 opts.flush_to = l2cap_pi(sk)->flush_to;
-                opts.mode     = L2CAP_MODE_BASIC;
+                opts.mode     = l2cap_pi(sk)->mode;
+                opts.fcs      = l2cap_pi(sk)->fcs;
 
                 len = min_t(unsigned int, sizeof(opts), optlen);
                 if (copy_from_user((char *) &opts, optval, len)) {
@@ -1265,8 +1723,10 @@ static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, char __us
                         break;
                 }
 
-                l2cap_pi(sk)->imtu  = opts.imtu;
-                l2cap_pi(sk)->omtu  = opts.omtu;
+                l2cap_pi(sk)->imtu = opts.imtu;
+                l2cap_pi(sk)->omtu = opts.omtu;
+                l2cap_pi(sk)->mode = opts.mode;
+                l2cap_pi(sk)->fcs  = opts.fcs;
                 break;
 
         case L2CAP_LM:
@@ -1379,7 +1839,8 @@ static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, char __us
                 opts.imtu     = l2cap_pi(sk)->imtu;
                 opts.omtu     = l2cap_pi(sk)->omtu;
                 opts.flush_to = l2cap_pi(sk)->flush_to;
-                opts.mode     = L2CAP_MODE_BASIC;
+                opts.mode     = l2cap_pi(sk)->mode;
+                opts.fcs      = l2cap_pi(sk)->fcs;
 
                 len = min_t(unsigned int, len, sizeof(opts));
                 if (copy_to_user(optval, (char *) &opts, len))
@@ -1708,16 +2169,108 @@ static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val)
         *ptr += L2CAP_CONF_OPT_SIZE + len;
 }
 
+static int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
+{
+        u32 local_feat_mask = l2cap_feat_mask;
+        if (enable_ertm)
+                local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING;
+
+        switch (mode) {
+        case L2CAP_MODE_ERTM:
+                return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask;
+        case L2CAP_MODE_STREAMING:
+                return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask;
+        default:
+                return 0x00;
+        }
+}
+
+static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask)
+{
+        switch (mode) {
+        case L2CAP_MODE_STREAMING:
+        case L2CAP_MODE_ERTM:
+                if (l2cap_mode_supported(mode, remote_feat_mask))
+                        return mode;
+                /* fall through */
+        default:
+                return L2CAP_MODE_BASIC;
+        }
+}
+
 static int l2cap_build_conf_req(struct sock *sk, void *data)
 {
         struct l2cap_pinfo *pi = l2cap_pi(sk);
         struct l2cap_conf_req *req = data;
+        struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_ERTM };
         void *ptr = req->data;
 
         BT_DBG("sk %p", sk);
 
-        if (pi->imtu != L2CAP_DEFAULT_MTU)
-                l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
+        if (pi->num_conf_req || pi->num_conf_rsp)
+                goto done;
+
+        switch (pi->mode) {
+        case L2CAP_MODE_STREAMING:
+        case L2CAP_MODE_ERTM:
+                pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
+                if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
+                        l2cap_send_disconn_req(pi->conn, sk);
+                break;
+        default:
+                pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
+                break;
+        }
+
+done:
+        switch (pi->mode) {
+        case L2CAP_MODE_BASIC:
+                if (pi->imtu != L2CAP_DEFAULT_MTU)
+                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu);
+                break;
+
+        case L2CAP_MODE_ERTM:
+                rfc.mode            = L2CAP_MODE_ERTM;
+                rfc.txwin_size      = L2CAP_DEFAULT_TX_WINDOW;
+                rfc.max_transmit    = L2CAP_DEFAULT_MAX_TX;
+                rfc.retrans_timeout = 0;
+                rfc.monitor_timeout = 0;
+                rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
+
+                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
+                                        sizeof(rfc), (unsigned long) &rfc);
+
+                if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
+                        break;
+
+                if (pi->fcs == L2CAP_FCS_NONE ||
+                                pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
+                        pi->fcs = L2CAP_FCS_NONE;
+                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
+                }
+                break;
+
+        case L2CAP_MODE_STREAMING:
+                rfc.mode            = L2CAP_MODE_STREAMING;
+                rfc.txwin_size      = 0;
+                rfc.max_transmit    = 0;
+                rfc.retrans_timeout = 0;
+                rfc.monitor_timeout = 0;
+                rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
+
+                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
+                                        sizeof(rfc), (unsigned long) &rfc);
+
+                if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS))
+                        break;
+
+                if (pi->fcs == L2CAP_FCS_NONE ||
+                                pi->conf_state & L2CAP_CONF_NO_FCS_RECV) {
+                        pi->fcs = L2CAP_FCS_NONE;
+                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs);
+                }
+                break;
+        }
 
         /* FIXME: Need actual value of the flush timeout */
         //if (flush_to != L2CAP_DEFAULT_FLUSH_TO)
@@ -1767,6 +2320,12 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data)
                                 memcpy(&rfc, (void *) val, olen);
                         break;
 
+                case L2CAP_CONF_FCS:
+                        if (val == L2CAP_FCS_NONE)
+                                pi->conf_state |= L2CAP_CONF_NO_FCS_RECV;
+
+                        break;
+
                 default:
                         if (hint)
                                 break;
@@ -1777,30 +2336,83 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data)
                 }
         }
 
+        if (pi->num_conf_rsp || pi->num_conf_req)
+                goto done;
+
+        switch (pi->mode) {
+        case L2CAP_MODE_STREAMING:
+        case L2CAP_MODE_ERTM:
+                pi->conf_state |= L2CAP_CONF_STATE2_DEVICE;
+                if (!l2cap_mode_supported(pi->mode, pi->conn->feat_mask))
+                        return -ECONNREFUSED;
+                break;
+        default:
+                pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask);
+                break;
+        }
+
+done:
+        if (pi->mode != rfc.mode) {
+                result = L2CAP_CONF_UNACCEPT;
+                rfc.mode = pi->mode;
+
+                if (pi->num_conf_rsp == 1)
+                        return -ECONNREFUSED;
+
+                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
+                                        sizeof(rfc), (unsigned long) &rfc);
+        }
+
+
         if (result == L2CAP_CONF_SUCCESS) {
                 /* Configure output options and let the other side know
                  * which ones we don't like. */
 
-                if (rfc.mode == L2CAP_MODE_BASIC) {
-                        if (mtu < pi->omtu)
-                                result = L2CAP_CONF_UNACCEPT;
-                        else {
-                                pi->omtu = mtu;
-                                pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
-                        }
+                if (mtu < L2CAP_DEFAULT_MIN_MTU)
+                        result = L2CAP_CONF_UNACCEPT;
+                else {
+                        pi->omtu = mtu;
+                        pi->conf_state |= L2CAP_CONF_MTU_DONE;
+                }
+                l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
 
-                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
-                } else {
+                switch (rfc.mode) {
+                case L2CAP_MODE_BASIC:
+                        pi->fcs = L2CAP_FCS_NONE;
+                        pi->conf_state |= L2CAP_CONF_MODE_DONE;
+                        break;
+
+                case L2CAP_MODE_ERTM:
+                        pi->remote_tx_win = rfc.txwin_size;
+                        pi->remote_max_tx = rfc.max_transmit;
+                        pi->max_pdu_size = rfc.max_pdu_size;
+
+                        rfc.retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
+                        rfc.monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
+
+                        pi->conf_state |= L2CAP_CONF_MODE_DONE;
+                        break;
+
+                case L2CAP_MODE_STREAMING:
+                        pi->remote_tx_win = rfc.txwin_size;
+                        pi->max_pdu_size = rfc.max_pdu_size;
+
+                        pi->conf_state |= L2CAP_CONF_MODE_DONE;
+                        break;
+
+                default:
                         result = L2CAP_CONF_UNACCEPT;
 
                         memset(&rfc, 0, sizeof(rfc));
-                        rfc.mode = L2CAP_MODE_BASIC;
-
-                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
-                                                sizeof(rfc), (unsigned long) &rfc);
+                        rfc.mode = pi->mode;
                 }
-        }
 
+                l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
+                                        sizeof(rfc), (unsigned long) &rfc);
+
+                if (result == L2CAP_CONF_SUCCESS)
+                        pi->conf_state |= L2CAP_CONF_OUTPUT_DONE;
+        }
         rsp->scid   = cpu_to_le16(pi->dcid);
         rsp->result = cpu_to_le16(result);
         rsp->flags  = cpu_to_le16(0x0000);
@@ -1808,6 +2420,73 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data)
         return ptr - data;
 }
 
+static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        struct l2cap_conf_req *req = data;
+        void *ptr = req->data;
+        int type, olen;
+        unsigned long val;
+        struct l2cap_conf_rfc rfc;
+
+        BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data);
+
+        while (len >= L2CAP_CONF_OPT_SIZE) {
+                len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val);
+
+                switch (type) {
+                case L2CAP_CONF_MTU:
+                        if (val < L2CAP_DEFAULT_MIN_MTU) {
+                                *result = L2CAP_CONF_UNACCEPT;
+                                pi->omtu = L2CAP_DEFAULT_MIN_MTU;
+                        } else
+                                pi->omtu = val;
+                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu);
+                        break;
+
+                case L2CAP_CONF_FLUSH_TO:
+                        pi->flush_to = val;
+                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO,
+                                                        2, pi->flush_to);
+                        break;
+
+                case L2CAP_CONF_RFC:
+                        if (olen == sizeof(rfc))
+                                memcpy(&rfc, (void *)val, olen);
+
+                        if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) &&
+                                                        rfc.mode != pi->mode)
+                                return -ECONNREFUSED;
+
+                        pi->mode = rfc.mode;
+                        pi->fcs = 0;
+
+                        l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
+                                        sizeof(rfc), (unsigned long) &rfc);
+                        break;
+                }
+        }
+
+        if (*result == L2CAP_CONF_SUCCESS) {
+                switch (rfc.mode) {
+                case L2CAP_MODE_ERTM:
+                        pi->remote_tx_win   = rfc.txwin_size;
+                        pi->retrans_timeout = rfc.retrans_timeout;
+                        pi->monitor_timeout = rfc.monitor_timeout;
+                        pi->max_pdu_size    = le16_to_cpu(rfc.max_pdu_size);
+                        break;
+                case L2CAP_MODE_STREAMING:
+                        pi->max_pdu_size    = le16_to_cpu(rfc.max_pdu_size);
+                        break;
+                }
+        }
+
+        req->dcid   = cpu_to_le16(pi->dcid);
+        req->flags  = cpu_to_le16(0x0000);
+
+        return ptr - data;
+}
+
 static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags)
 {
         struct l2cap_conf_rsp *rsp = data;
@@ -1994,6 +2673,7 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
 
                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
                                         l2cap_build_conf_req(sk, req), req);
+                l2cap_pi(sk)->num_conf_req++;
                 break;
 
         case L2CAP_CR_PEND:
@@ -2052,10 +2732,13 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 
         /* Complete config. */
         len = l2cap_parse_conf_req(sk, rsp);
-        if (len < 0)
+        if (len < 0) {
+                l2cap_send_disconn_req(conn, sk);
                 goto unlock;
+        }
 
         l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp);
+        l2cap_pi(sk)->num_conf_rsp++;
 
         /* Reset config buffer. */
         l2cap_pi(sk)->conf_len = 0;
@@ -2064,7 +2747,22 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                 goto unlock;
 
         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
+                if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV)
+                                || l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
+                        l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
+
                 sk->sk_state = BT_CONNECTED;
+                l2cap_pi(sk)->next_tx_seq = 0;
+                l2cap_pi(sk)->expected_ack_seq = 0;
+                l2cap_pi(sk)->unacked_frames = 0;
+
+                setup_timer(&l2cap_pi(sk)->retrans_timer,
+                                l2cap_retrans_timeout, (unsigned long) sk);
+                setup_timer(&l2cap_pi(sk)->monitor_timer,
+                                l2cap_monitor_timeout, (unsigned long) sk);
+
+                __skb_queue_head_init(TX_QUEUE(sk));
+                __skb_queue_head_init(SREJ_QUEUE(sk));
                 l2cap_chan_ready(sk);
                 goto unlock;
         }
@@ -2073,6 +2771,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                 u8 buf[64];
                 l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
                                         l2cap_build_conf_req(sk, buf), buf);
+                l2cap_pi(sk)->num_conf_req++;
         }
 
 unlock:
@@ -2102,29 +2801,32 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
                 break;
 
         case L2CAP_CONF_UNACCEPT:
-                if (++l2cap_pi(sk)->conf_retry < L2CAP_CONF_MAX_RETRIES) {
-                        char req[128];
-                        /* It does not make sense to adjust L2CAP parameters
-                         * that are currently defined in the spec. We simply
-                         * resend config request that we sent earlier. It is
-                         * stupid, but it helps qualification testing which
-                         * expects at least some response from us. */
-                        l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ,
-                                                l2cap_build_conf_req(sk, req), req);
-                        goto done;
+                if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) {
+                        int len = cmd->len - sizeof(*rsp);
+                        char req[64];
+
+                        /* throw out any old stored conf requests */
+                        result = L2CAP_CONF_SUCCESS;
+                        len = l2cap_parse_conf_rsp(sk, rsp->data,
+                                                        len, req, &result);
+                        if (len < 0) {
+                                l2cap_send_disconn_req(conn, sk);
+                                goto done;
+                        }
+
+                        l2cap_send_cmd(conn, l2cap_get_ident(conn),
+                                                L2CAP_CONF_REQ, len, req);
+                        l2cap_pi(sk)->num_conf_req++;
+                        if (result != L2CAP_CONF_SUCCESS)
+                                goto done;
+                        break;
                 }
 
         default:
                 sk->sk_state = BT_DISCONN;
                 sk->sk_err = ECONNRESET;
                 l2cap_sock_set_timer(sk, HZ * 5);
-                {
-                        struct l2cap_disconn_req req;
-                        req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid);
-                        req.scid = cpu_to_le16(l2cap_pi(sk)->scid);
-                        l2cap_send_cmd(conn, l2cap_get_ident(conn),
-                                        L2CAP_DISCONN_REQ, sizeof(req), &req);
-                }
+                l2cap_send_disconn_req(conn, sk);
                 goto done;
         }
 
@@ -2134,7 +2836,16 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
         l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE;
 
         if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
+                if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_NO_FCS_RECV)
+                                || l2cap_pi(sk)->fcs != L2CAP_FCS_NONE)
+                        l2cap_pi(sk)->fcs = L2CAP_FCS_CRC16;
+
                 sk->sk_state = BT_CONNECTED;
+                l2cap_pi(sk)->expected_tx_seq = 0;
+                l2cap_pi(sk)->buffer_seq = 0;
+                l2cap_pi(sk)->num_to_ack = 0;
+                __skb_queue_head_init(TX_QUEUE(sk));
+                __skb_queue_head_init(SREJ_QUEUE(sk));
                 l2cap_chan_ready(sk);
         }
 
@@ -2165,6 +2876,11 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
 
         sk->sk_shutdown = SHUTDOWN_MASK;
 
+        skb_queue_purge(TX_QUEUE(sk));
+        skb_queue_purge(SREJ_QUEUE(sk));
+        del_timer(&l2cap_pi(sk)->retrans_timer);
+        del_timer(&l2cap_pi(sk)->monitor_timer);
+
         l2cap_chan_del(sk, ECONNRESET);
         bh_unlock_sock(sk);
 
@@ -2187,6 +2903,11 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
         if (!sk)
                 return 0;
 
+        skb_queue_purge(TX_QUEUE(sk));
+        skb_queue_purge(SREJ_QUEUE(sk));
+        del_timer(&l2cap_pi(sk)->retrans_timer);
+        del_timer(&l2cap_pi(sk)->monitor_timer);
+
         l2cap_chan_del(sk, 0);
         bh_unlock_sock(sk);
 
@@ -2205,10 +2926,14 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm
 
         if (type == L2CAP_IT_FEAT_MASK) {
                 u8 buf[8];
+                u32 feat_mask = l2cap_feat_mask;
                 struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf;
                 rsp->type   = cpu_to_le16(L2CAP_IT_FEAT_MASK);
                 rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS);
-                put_unaligned(cpu_to_le32(l2cap_feat_mask), (__le32 *) rsp->data);
+                if (enable_ertm)
+                        feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING
+                                                         | L2CAP_FEAT_FCS;
+                put_unaligned_le32(feat_mask, rsp->data);
                 l2cap_send_cmd(conn, cmd->ident,
                                         L2CAP_INFO_RSP, sizeof(buf), buf);
         } else if (type == L2CAP_IT_FIXED_CHAN) {
@@ -2359,9 +3084,374 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
         kfree_skb(skb);
 }
 
+static int l2cap_check_fcs(struct l2cap_pinfo *pi,  struct sk_buff *skb)
+{
+        u16 our_fcs, rcv_fcs;
+        int hdr_size = L2CAP_HDR_SIZE + 2;
+
+        if (pi->fcs == L2CAP_FCS_CRC16) {
+                skb_trim(skb, skb->len - 2);
+                rcv_fcs = get_unaligned_le16(skb->data + skb->len);
+                our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size);
+
+                if (our_fcs != rcv_fcs)
+                        return -EINVAL;
+        }
+        return 0;
+}
+
+static void l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
+{
+        struct sk_buff *next_skb;
+
+        bt_cb(skb)->tx_seq = tx_seq;
+        bt_cb(skb)->sar = sar;
+
+        next_skb = skb_peek(SREJ_QUEUE(sk));
+        if (!next_skb) {
+                __skb_queue_tail(SREJ_QUEUE(sk), skb);
+                return;
+        }
+
+        do {
+                if (bt_cb(next_skb)->tx_seq > tx_seq) {
+                        __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
+                        return;
+                }
+
+                if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
+                        break;
+
+        } while((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
+
+        __skb_queue_tail(SREJ_QUEUE(sk), skb);
+}
+
+static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        struct sk_buff *_skb;
+        int err = -EINVAL;
+
+        switch (control & L2CAP_CTRL_SAR) {
+        case L2CAP_SDU_UNSEGMENTED:
+                if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
+                        kfree_skb(pi->sdu);
+                        break;
+                }
+
+                err = sock_queue_rcv_skb(sk, skb);
+                if (!err)
+                        return 0;
+
+                break;
+
+        case L2CAP_SDU_START:
+                if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
+                        kfree_skb(pi->sdu);
+                        break;
+                }
+
+                pi->sdu_len = get_unaligned_le16(skb->data);
+                skb_pull(skb, 2);
+
+                pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
+                if (!pi->sdu) {
+                        err = -ENOMEM;
+                        break;
+                }
+
+                memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
+
+                pi->conn_state |= L2CAP_CONN_SAR_SDU;
+                pi->partial_sdu_len = skb->len;
+                err = 0;
+                break;
+
+        case L2CAP_SDU_CONTINUE:
+                if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
+                        break;
+
+                memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
+
+                pi->partial_sdu_len += skb->len;
+                if (pi->partial_sdu_len > pi->sdu_len)
+                        kfree_skb(pi->sdu);
+                else
+                        err = 0;
+
+                break;
+
+        case L2CAP_SDU_END:
+                if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
+                        break;
+
+                memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
+
+                pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
+                pi->partial_sdu_len += skb->len;
+
+                if (pi->partial_sdu_len == pi->sdu_len) {
+                        _skb = skb_clone(pi->sdu, GFP_ATOMIC);
+                        err = sock_queue_rcv_skb(sk, _skb);
+                        if (err < 0)
+                                kfree_skb(_skb);
+                }
+                kfree_skb(pi->sdu);
+                err = 0;
+
+                break;
+        }
+
+        kfree_skb(skb);
+        return err;
+}
+
+static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
+{
+        struct sk_buff *skb;
+        u16 control = 0;
+
+        while((skb = skb_peek(SREJ_QUEUE(sk)))) {
+                if (bt_cb(skb)->tx_seq != tx_seq)
+                        break;
+
+                skb = skb_dequeue(SREJ_QUEUE(sk));
+                control |= bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
+                l2cap_sar_reassembly_sdu(sk, skb, control);
+                l2cap_pi(sk)->buffer_seq_srej =
+                        (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
+                tx_seq++;
+        }
+}
+
+static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        struct srej_list *l, *tmp;
+        u16 control;
+
+        list_for_each_entry_safe(l,tmp, SREJ_LIST(sk), list) {
+                if (l->tx_seq == tx_seq) {
+                        list_del(&l->list);
+                        kfree(l);
+                        return;
+                }
+                control = L2CAP_SUPER_SELECT_REJECT;
+                control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+                l2cap_send_sframe(pi, control);
+                list_del(&l->list);
+                list_add_tail(&l->list, SREJ_LIST(sk));
+        }
+}
+
+static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        struct srej_list *new;
+        u16 control;
+
+        while (tx_seq != pi->expected_tx_seq) {
+                control = L2CAP_SUPER_SELECT_REJECT;
+                control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+                if (pi->conn_state & L2CAP_CONN_SEND_PBIT) {
+                        control |= L2CAP_CTRL_POLL;
+                        pi->conn_state &= ~L2CAP_CONN_SEND_PBIT;
+                }
+                l2cap_send_sframe(pi, control);
+
+                new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
+                new->tx_seq = pi->expected_tx_seq++;
+                list_add_tail(&new->list, SREJ_LIST(sk));
+        }
+        pi->expected_tx_seq++;
+}
+
+static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        u8 tx_seq = __get_txseq(rx_control);
+        u16 tx_control = 0;
+        u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
+        int err = 0;
+
+        BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
+
+        if (tx_seq == pi->expected_tx_seq)
+                goto expected;
+
+        if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
+                struct srej_list *first;
+
+                first = list_first_entry(SREJ_LIST(sk),
+                                struct srej_list, list);
+                if (tx_seq == first->tx_seq) {
+                        l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
+                        l2cap_check_srej_gap(sk, tx_seq);
+
+                        list_del(&first->list);
+                        kfree(first);
+
+                        if (list_empty(SREJ_LIST(sk))) {
+                                pi->buffer_seq = pi->buffer_seq_srej;
+                                pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
+                        }
+                } else {
+                        struct srej_list *l;
+                        l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
+
+                        list_for_each_entry(l, SREJ_LIST(sk), list) {
+                                if (l->tx_seq == tx_seq) {
+                                        l2cap_resend_srejframe(sk, tx_seq);
+                                        return 0;
+                                }
+                        }
+                        l2cap_send_srejframe(sk, tx_seq);
+                }
+        } else {
+                pi->conn_state |= L2CAP_CONN_SREJ_SENT;
+
+                INIT_LIST_HEAD(SREJ_LIST(sk));
+                pi->buffer_seq_srej = pi->buffer_seq;
+
+                __skb_queue_head_init(SREJ_QUEUE(sk));
+                l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
+
+                pi->conn_state |= L2CAP_CONN_SEND_PBIT;
+
+                l2cap_send_srejframe(sk, tx_seq);
+        }
+        return 0;
+
+expected:
+        pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
+
+        if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
+                l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
+                return 0;
+        }
+
+        pi->buffer_seq = (pi->buffer_seq + 1) % 64;
+
+        err = l2cap_sar_reassembly_sdu(sk, skb, rx_control);
+        if (err < 0)
+                return err;
+
+        pi->num_to_ack = (pi->num_to_ack + 1) % L2CAP_DEFAULT_NUM_TO_ACK;
+        if (pi->num_to_ack == L2CAP_DEFAULT_NUM_TO_ACK - 1) {
+                tx_control |= L2CAP_SUPER_RCV_READY;
+                tx_control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+                l2cap_send_sframe(pi, tx_control);
+        }
+        return 0;
+}
+
+static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
+{
+        struct l2cap_pinfo *pi = l2cap_pi(sk);
+        u8 tx_seq = __get_reqseq(rx_control);
+
+        BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
+
+        switch (rx_control & L2CAP_CTRL_SUPERVISE) {
+        case L2CAP_SUPER_RCV_READY:
+                if (rx_control & L2CAP_CTRL_POLL) {
+                        u16 control = L2CAP_CTRL_FINAL;
+                        control |= L2CAP_SUPER_RCV_READY |
+                                (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT);
+                        l2cap_send_sframe(l2cap_pi(sk), control);
+                        pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+
+                } else if (rx_control & L2CAP_CTRL_FINAL) {
+                        pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+                        pi->expected_ack_seq = tx_seq;
+                        l2cap_drop_acked_frames(sk);
+
+                        if (!(pi->conn_state & L2CAP_CONN_WAIT_F))
+                                break;
+
+                        pi->conn_state &= ~L2CAP_CONN_WAIT_F;
+                        del_timer(&pi->monitor_timer);
+
+                        if (pi->unacked_frames > 0)
+                                __mod_retrans_timer();
+                } else {
+                        pi->expected_ack_seq = tx_seq;
+                        l2cap_drop_acked_frames(sk);
+
+                        if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY)
+                                        && (pi->unacked_frames > 0))
+                                __mod_retrans_timer();
+
+                        l2cap_ertm_send(sk);
+                        pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+                }
+                break;
+
+        case L2CAP_SUPER_REJECT:
+                pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+
+                pi->expected_ack_seq = __get_reqseq(rx_control);
+                l2cap_drop_acked_frames(sk);
+
+                sk->sk_send_head = TX_QUEUE(sk)->next;
+                pi->next_tx_seq = pi->expected_ack_seq;
+
+                l2cap_ertm_send(sk);
+
+                break;
+
+        case L2CAP_SUPER_SELECT_REJECT:
+                pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+
+                if (rx_control & L2CAP_CTRL_POLL) {
+                        l2cap_retransmit_frame(sk, tx_seq);
+                        pi->expected_ack_seq = tx_seq;
+                        l2cap_drop_acked_frames(sk);
+                        l2cap_ertm_send(sk);
+                        if (pi->conn_state & L2CAP_CONN_WAIT_F) {
+                                pi->srej_save_reqseq = tx_seq;
+                                pi->conn_state |= L2CAP_CONN_SREJ_ACT;
+                        }
+                } else if (rx_control & L2CAP_CTRL_FINAL) {
+                        if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
+                                        pi->srej_save_reqseq == tx_seq)
+                                pi->srej_save_reqseq &= ~L2CAP_CONN_SREJ_ACT;
+                        else
+                                l2cap_retransmit_frame(sk, tx_seq);
+                }
+                else {
+                        l2cap_retransmit_frame(sk, tx_seq);
+                        if (pi->conn_state & L2CAP_CONN_WAIT_F) {
+                                pi->srej_save_reqseq = tx_seq;
+                                pi->conn_state |= L2CAP_CONN_SREJ_ACT;
+                        }
+                }
+                break;
+
+        case L2CAP_SUPER_RCV_NOT_READY:
+                pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
+                pi->expected_ack_seq = tx_seq;
+                l2cap_drop_acked_frames(sk);
+
+                del_timer(&l2cap_pi(sk)->retrans_timer);
+                if (rx_control & L2CAP_CTRL_POLL) {
+                        u16 control = L2CAP_CTRL_FINAL;
+                        l2cap_send_rr_or_rnr(l2cap_pi(sk), control);
+                }
+                break;
+        }
+
+        return 0;
+}
+
 static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
 {
         struct sock *sk;
+        struct l2cap_pinfo *pi;
+        u16 control, len;
+        u8 tx_seq;
+        int err;
 
         sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
         if (!sk) {
@@ -2369,22 +3459,91 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
                 goto drop;
         }
 
+        pi = l2cap_pi(sk);
+
         BT_DBG("sk %p, len %d", sk, skb->len);
 
         if (sk->sk_state != BT_CONNECTED)
                 goto drop;
 
-        if (l2cap_pi(sk)->imtu < skb->len)
-                goto drop;
+        switch (pi->mode) {
+        case L2CAP_MODE_BASIC:
+                /* If socket recv buffers overflows we drop data here
+                 * which is *bad* because L2CAP has to be reliable.
+                 * But we don't have any other choice. L2CAP doesn't
+                 * provide flow control mechanism. */
 
-        /* If socket recv buffers overflows we drop data here
-         * which is *bad* because L2CAP has to be reliable.
-         * But we don't have any other choice. L2CAP doesn't
-         * provide flow control mechanism. */
+                if (pi->imtu < skb->len)
+                        goto drop;
+
+                if (!sock_queue_rcv_skb(sk, skb))
+                        goto done;
+                break;
+
+        case L2CAP_MODE_ERTM:
+                control = get_unaligned_le16(skb->data);
+                skb_pull(skb, 2);
+                len = skb->len;
+
+                if (__is_sar_start(control))
+                        len -= 2;
+
+                if (pi->fcs == L2CAP_FCS_CRC16)
+                        len -= 2;
+
+                /*
+                 * We can just drop the corrupted I-frame here.
+                 * Receiver will miss it and start proper recovery
+                 * procedures and ask retransmission.
+                 */
+                if (len > L2CAP_DEFAULT_MAX_PDU_SIZE)
+                        goto drop;
+
+                if (l2cap_check_fcs(pi, skb))
+                        goto drop;
+
+                if (__is_iframe(control))
+                        err = l2cap_data_channel_iframe(sk, control, skb);
+                else
+                        err = l2cap_data_channel_sframe(sk, control, skb);
+
+                if (!err)
+                        goto done;
+                break;
+
+        case L2CAP_MODE_STREAMING:
+                control = get_unaligned_le16(skb->data);
+                skb_pull(skb, 2);
+                len = skb->len;
+
+                if (__is_sar_start(control))
+                        len -= 2;
+
+                if (pi->fcs == L2CAP_FCS_CRC16)
+                        len -= 2;
+
+                if (len > L2CAP_DEFAULT_MAX_PDU_SIZE || __is_sframe(control))
+                        goto drop;
+
+                if (l2cap_check_fcs(pi, skb))
+                        goto drop;
+
+                tx_seq = __get_txseq(control);
+
+                if (pi->expected_tx_seq == tx_seq)
+                        pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
+                else
+                        pi->expected_tx_seq = tx_seq + 1;
+
+                err = l2cap_sar_reassembly_sdu(sk, skb, control);
 
-        if (!sock_queue_rcv_skb(sk, skb))
                 goto done;
 
+        default:
+                BT_DBG("sk %p: bad mode 0x%2.2x", sk, l2cap_pi(sk)->mode);
+                break;
+        }
+
 drop:
         kfree_skb(skb);
 
@@ -2433,6 +3592,11 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
         cid = __le16_to_cpu(lh->cid);
         len = __le16_to_cpu(lh->len);
 
+        if (len != skb->len) {
+                kfree_skb(skb);
+                return;
+        }
+
         BT_DBG("len %d, cid 0x%4.4x", len, cid);
 
         switch (cid) {
@@ -2441,7 +3605,7 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
                 break;
 
         case L2CAP_CID_CONN_LESS:
-                psm = get_unaligned((__le16 *) skb->data);
+                psm = get_unaligned_le16(skb->data);
                 skb_pull(skb, 2);
                 l2cap_conless_channel(conn, psm, skb);
                 break;
@@ -2828,6 +3992,9 @@ EXPORT_SYMBOL(l2cap_load);
 module_init(l2cap_init);
 module_exit(l2cap_exit);
 
+module_param(enable_ertm, bool, 0644);
+MODULE_PARM_DESC(enable_ertm, "Enable enhanced retransmission mode");
+
 MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>");
 MODULE_DESCRIPTION("Bluetooth L2CAP ver " VERSION);
 MODULE_VERSION(VERSION);
diff --git a/net/bluetooth/rfcomm/core.c b/net/bluetooth/rfcomm/core.c
index 94b3388c188b..25692bc0a342 100644
--- a/net/bluetooth/rfcomm/core.c
+++ b/net/bluetooth/rfcomm/core.c
@@ -244,6 +244,33 @@ static inline int rfcomm_check_security(struct rfcomm_dlc *d)
                                                                 auth_type);
 }
 
+static void rfcomm_session_timeout(unsigned long arg)
+{
+        struct rfcomm_session *s = (void *) arg;
+
+        BT_DBG("session %p state %ld", s, s->state);
+
+        set_bit(RFCOMM_TIMED_OUT, &s->flags);
+        rfcomm_session_put(s);
+        rfcomm_schedule(RFCOMM_SCHED_TIMEO);
+}
+
+static void rfcomm_session_set_timer(struct rfcomm_session *s, long timeout)
+{
+        BT_DBG("session %p state %ld timeout %ld", s, s->state, timeout);
+
+        if (!mod_timer(&s->timer, jiffies + timeout))
+                rfcomm_session_hold(s);
+}
+
+static void rfcomm_session_clear_timer(struct rfcomm_session *s)
+{
+        BT_DBG("session %p state %ld", s, s->state);
+
+        if (timer_pending(&s->timer) && del_timer(&s->timer))
+                rfcomm_session_put(s);
+}
+
 /* ---- RFCOMM DLCs ---- */
 static void rfcomm_dlc_timeout(unsigned long arg)
 {
@@ -320,6 +347,7 @@ static void rfcomm_dlc_link(struct rfcomm_session *s, struct rfcomm_dlc *d)
 
         rfcomm_session_hold(s);
 
+        rfcomm_session_clear_timer(s);
         rfcomm_dlc_hold(d);
         list_add(&d->list, &s->dlcs);
         d->session = s;
@@ -335,6 +363,9 @@ static void rfcomm_dlc_unlink(struct rfcomm_dlc *d)
         d->session = NULL;
         rfcomm_dlc_put(d);
 
+        if (list_empty(&s->dlcs))
+                rfcomm_session_set_timer(s, RFCOMM_IDLE_TIMEOUT);
+
         rfcomm_session_put(s);
 }
 
@@ -567,6 +598,8 @@ static struct rfcomm_session *rfcomm_session_add(struct socket *sock, int state)
 
         BT_DBG("session %p sock %p", s, sock);
 
+        setup_timer(&s->timer, rfcomm_session_timeout, (unsigned long) s);
+
         INIT_LIST_HEAD(&s->dlcs);
         s->state = state;
         s->sock  = sock;
@@ -598,6 +631,7 @@ static void rfcomm_session_del(struct rfcomm_session *s)
         if (state == BT_CONNECTED)
                 rfcomm_send_disc(s, 0);
 
+        rfcomm_session_clear_timer(s);
         sock_release(s->sock);
         kfree(s);
 
@@ -639,6 +673,7 @@ static void rfcomm_session_close(struct rfcomm_session *s, int err)
                 __rfcomm_dlc_close(d, err);
         }
 
+        rfcomm_session_clear_timer(s);
         rfcomm_session_put(s);
 }
 
@@ -1879,6 +1914,12 @@ static inline void rfcomm_process_sessions(void)
                 struct rfcomm_session *s;
                 s = list_entry(p, struct rfcomm_session, list);
 
+                if (test_and_clear_bit(RFCOMM_TIMED_OUT, &s->flags)) {
+                        s->state = BT_DISCONN;
+                        rfcomm_send_disc(s, 0);
+                        continue;
+                }
+
                 if (s->state == BT_LISTEN) {
                         rfcomm_accept_connection(s);
                         continue;
@@ -2080,7 +2121,7 @@ static CLASS_ATTR(rfcomm_dlc, S_IRUGO, rfcomm_dlc_sysfs_show, NULL);
 /* ---- Initialization ---- */
 static int __init rfcomm_init(void)
 {
-        int ret;
+        int err;
 
         l2cap_load();
 
@@ -2088,33 +2129,35 @@ static int __init rfcomm_init(void)
 
         rfcomm_thread = kthread_run(rfcomm_run, NULL, "krfcommd");
         if (IS_ERR(rfcomm_thread)) {
-                ret = PTR_ERR(rfcomm_thread);
-                goto out_thread;
+                err = PTR_ERR(rfcomm_thread);
+                goto unregister;
         }
 
         if (class_create_file(bt_class, &class_attr_rfcomm_dlc) < 0)
                 BT_ERR("Failed to create RFCOMM info file");
 
-        ret = rfcomm_init_ttys();
-        if (ret)
-                goto out_tty;
+        err = rfcomm_init_ttys();
+        if (err < 0)
+                goto stop;
 
-        ret = rfcomm_init_sockets();
-        if (ret)
-                goto out_sock;
+        err = rfcomm_init_sockets();
+        if (err < 0)
+                goto cleanup;
 
         BT_INFO("RFCOMM ver %s", VERSION);
 
         return 0;
 
-out_sock:
+cleanup:
         rfcomm_cleanup_ttys();
-out_tty:
+
+stop:
         kthread_stop(rfcomm_thread);
-out_thread:
+
+unregister:
         hci_unregister_cb(&rfcomm_cb);
 
-        return ret;
+        return err;
 }
 
 static void __exit rfcomm_exit(void)
diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c
index 51ae0c3e470a..13c27f17192c 100644
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -359,20 +359,9 @@ static void sco_sock_kill(struct sock *sk)
         sock_put(sk);
 }
 
-/* Close socket.
- * Must be called on unlocked socket.
- */
-static void sco_sock_close(struct sock *sk)
+static void __sco_sock_close(struct sock *sk)
 {
-        struct sco_conn *conn;
-
-        sco_sock_clear_timer(sk);
-
-        lock_sock(sk);
-
-        conn = sco_pi(sk)->conn;
-
-        BT_DBG("sk %p state %d conn %p socket %p", sk, sk->sk_state, conn, sk->sk_socket);
+        BT_DBG("sk %p state %d socket %p", sk, sk->sk_state, sk->sk_socket);
 
         switch (sk->sk_state) {
         case BT_LISTEN:
@@ -390,9 +379,15 @@ static void sco_sock_close(struct sock *sk)
                 sock_set_flag(sk, SOCK_ZAPPED);
                 break;
         }
+}
 
+/* Must be called on unlocked socket. */
+static void sco_sock_close(struct sock *sk)
+{
+        sco_sock_clear_timer(sk);
+        lock_sock(sk);
+        __sco_sock_close(sk);
         release_sock(sk);
-
         sco_sock_kill(sk);
 }
 
@@ -748,6 +743,30 @@ static int sco_sock_getsockopt(struct socket *sock, int level, int optname, char
         return err;
 }
 
+static int sco_sock_shutdown(struct socket *sock, int how)
+{
+        struct sock *sk = sock->sk;
+        int err = 0;
+
+        BT_DBG("sock %p, sk %p", sock, sk);
+
+        if (!sk)
+                return 0;
+
+        lock_sock(sk);
+        if (!sk->sk_shutdown) {
+                sk->sk_shutdown = SHUTDOWN_MASK;
+                sco_sock_clear_timer(sk);
+                __sco_sock_close(sk);
+
+                if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime)
+                        err = bt_sock_wait_state(sk, BT_CLOSED,
+                                                        sk->sk_lingertime);
+        }
+        release_sock(sk);
+        return err;
+}
+
 static int sco_sock_release(struct socket *sock)
 {
         struct sock *sk = sock->sk;
@@ -969,7 +988,7 @@ static const struct proto_ops sco_sock_ops = {
         .ioctl          = bt_sock_ioctl,
         .mmap           = sock_no_mmap,
         .socketpair     = sock_no_socketpair,
-        .shutdown       = sock_no_shutdown,
+        .shutdown       = sco_sock_shutdown,
         .setsockopt     = sco_sock_setsockopt,
         .getsockopt     = sco_sock_getsockopt
 };