233 files changed, 16075 insertions, 4551 deletions

diff --git a/drivers/acpi/Kconfig b/drivers/acpi/Kconfig
index 431f8b439553..7ec7d88c5999 100644
--- a/drivers/acpi/Kconfig
+++ b/drivers/acpi/Kconfig
@@ -266,6 +266,7 @@ config ACPI_DEBUG_FUNC_TRACE
 
 config ACPI_PCI_SLOT
         tristate "PCI slot detection driver"
+        depends on SYSFS
         default n
         help
           This driver creates entries in /sys/bus/pci/slots/ for all PCI
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index 2aa1908e5ce0..b17c57f85032 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -679,6 +679,14 @@ config PATA_PLATFORM
 
           If unsure, say N.
 
+config PATA_AT91
+        tristate "PATA support for AT91SAM9260"
+        depends on ARM && ARCH_AT91
+        help
+          This option enables support for IDE devices on the Atmel AT91SAM9260 SoC.
+
+          If unsure, say N.
+
 config PATA_OF_PLATFORM
         tristate "OpenFirmware platform device PATA support"
         depends on PATA_PLATFORM && PPC_OF
diff --git a/drivers/ata/Makefile b/drivers/ata/Makefile
index 1558059874f0..38906f9bbb4e 100644
--- a/drivers/ata/Makefile
+++ b/drivers/ata/Makefile
@@ -72,6 +72,7 @@ obj-$(CONFIG_PATA_SCH)		+= pata_sch.o
 obj-$(CONFIG_PATA_BF54X)        += pata_bf54x.o
 obj-$(CONFIG_PATA_OCTEON_CF)    += pata_octeon_cf.o
 obj-$(CONFIG_PATA_PLATFORM)     += pata_platform.o
+obj-$(CONFIG_PATA_AT91) += pata_at91.o
 obj-$(CONFIG_PATA_OF_PLATFORM)  += pata_of_platform.o
 obj-$(CONFIG_PATA_ICSIDE)       += pata_icside.o
 # Should be last but two libata driver
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index ca4d208ddf3b..045a486a09ea 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -125,19 +125,19 @@ MODULE_PARM_DESC(force, "Force ATA configurations including cable type, link spe
 
 static int atapi_enabled = 1;
 module_param(atapi_enabled, int, 0444);
-MODULE_PARM_DESC(atapi_enabled, "Enable discovery of ATAPI devices (0=off, 1=on)");
+MODULE_PARM_DESC(atapi_enabled, "Enable discovery of ATAPI devices (0=off, 1=on [default])");
 
 static int atapi_dmadir = 0;
 module_param(atapi_dmadir, int, 0444);
-MODULE_PARM_DESC(atapi_dmadir, "Enable ATAPI DMADIR bridge support (0=off, 1=on)");
+MODULE_PARM_DESC(atapi_dmadir, "Enable ATAPI DMADIR bridge support (0=off [default], 1=on)");
 
 int atapi_passthru16 = 1;
 module_param(atapi_passthru16, int, 0444);
-MODULE_PARM_DESC(atapi_passthru16, "Enable ATA_16 passthru for ATAPI devices; on by default (0=off, 1=on)");
+MODULE_PARM_DESC(atapi_passthru16, "Enable ATA_16 passthru for ATAPI devices (0=off, 1=on [default])");
 
 int libata_fua = 0;
 module_param_named(fua, libata_fua, int, 0444);
-MODULE_PARM_DESC(fua, "FUA support (0=off, 1=on)");
+MODULE_PARM_DESC(fua, "FUA support (0=off [default], 1=on)");
 
 static int ata_ignore_hpa;
 module_param_named(ignore_hpa, ata_ignore_hpa, int, 0644);
@@ -153,11 +153,11 @@ MODULE_PARM_DESC(ata_probe_timeout, "Set ATA probing timeout (seconds)");
 
 int libata_noacpi = 0;
 module_param_named(noacpi, libata_noacpi, int, 0444);
-MODULE_PARM_DESC(noacpi, "Disables the use of ACPI in probe/suspend/resume when set");
+MODULE_PARM_DESC(noacpi, "Disable the use of ACPI in probe/suspend/resume (0=off [default], 1=on)");
 
 int libata_allow_tpm = 0;
 module_param_named(allow_tpm, libata_allow_tpm, int, 0444);
-MODULE_PARM_DESC(allow_tpm, "Permit the use of TPM commands");
+MODULE_PARM_DESC(allow_tpm, "Permit the use of TPM commands (0=off [default], 1=on)");
 
 MODULE_AUTHOR("Jeff Garzik");
 MODULE_DESCRIPTION("Library module for ATA devices");
@@ -1993,11 +1993,17 @@ unsigned int ata_do_simple_cmd(struct ata_device *dev, u8 cmd)
  *      Check if the current speed of the device requires IORDY. Used
  *      by various controllers for chip configuration.
  */
-
 unsigned int ata_pio_need_iordy(const struct ata_device *adev)
 {
-        /* Controller doesn't support  IORDY. Probably a pointless check
-           as the caller should know this */
+        /* Don't set IORDY if we're preparing for reset.  IORDY may
+         * lead to controller lock up on certain controllers if the
+         * port is not occupied.  See bko#11703 for details.
+         */
+        if (adev->link->ap->pflags & ATA_PFLAG_RESETTING)
+                return 0;
+        /* Controller doesn't support IORDY.  Probably a pointless
+         * check as the caller should know this.
+         */
         if (adev->link->ap->flags & ATA_FLAG_NO_IORDY)
                 return 0;
         /* CF spec. r4.1 Table 22 says no iordy on PIO5 and PIO6.  */
@@ -2020,7 +2026,6 @@ unsigned int ata_pio_need_iordy(const struct ata_device *adev)
  *      Compute the highest mode possible if we are not using iordy. Return
  *      -1 if no iordy mode is available.
  */
-
 static u32 ata_pio_mask_no_iordy(const struct ata_device *adev)
 {
         /* If we have no drive specific rule, then PIO 2 is non IORDY */
diff --git a/drivers/ata/pata_at91.c b/drivers/ata/pata_at91.c
new file mode 100644
index 000000000000..4b27617be26d
--- /dev/null
+++ b/drivers/ata/pata_at91.c
@@ -0,0 +1,361 @@
+/*
+ * PATA driver for AT91SAM9260 Static Memory Controller
+ * with CompactFlash interface in True IDE mode
+ *
+ * Copyright (C) 2009 Matyukevich Sergey
+ *
+ * Based on:
+ *      * generic platform driver by Paul Mundt: drivers/ata/pata_platform.c
+ *      * pata_at32 driver by Kristoffer Nyborg Gregertsen
+ *      * at91_ide driver by Stanislaw Gruszka
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/blkdev.h>
+#include <scsi/scsi_host.h>
+#include <linux/ata.h>
+#include <linux/clk.h>
+#include <linux/libata.h>
+#include <linux/platform_device.h>
+#include <linux/ata_platform.h>
+
+#include <mach/at91sam9260_matrix.h>
+#include <mach/at91sam9_smc.h>
+#include <mach/at91sam9260.h>
+#include <mach/board.h>
+#include <mach/gpio.h>
+
+
+#define DRV_NAME "pata_at91"
+#define DRV_VERSION "0.1"
+
+#define CF_IDE_OFFSET       0x00c00000
+#define CF_ALT_IDE_OFFSET   0x00e00000
+#define CF_IDE_RES_SIZE     0x08
+
+struct at91_ide_info {
+        unsigned long mode;
+        unsigned int cs;
+
+        void __iomem *ide_addr;
+        void __iomem *alt_addr;
+};
+
+const struct ata_timing initial_timing =
+        {XFER_PIO_0, 70, 290, 240, 600, 165, 150, 600, 0};
+
+static unsigned int calc_mck_cycles(unsigned int ns, unsigned int mck_hz)
+{
+        unsigned long mul;
+
+    /*
+     * cycles = x [nsec] * f [Hz] / 10^9 [ns in sec] =
+     *     x * (f / 1_000_000_000) =
+     *     x * ((f * 65536) / 1_000_000_000) / 65536 =
+     *     x * (((f / 10_000) * 65536) / 100_000) / 65536 =
+     */
+
+    mul = (mck_hz / 10000) << 16;
+    mul /= 100000;
+
+    return (ns * mul + 65536) >> 16;    /* rounding */
+}
+
+static void set_smc_mode(struct at91_ide_info *info)
+{
+    at91_sys_write(AT91_SMC_MODE(info->cs), info->mode);
+    return;
+}
+
+static void set_smc_timing(struct device *dev,
+                struct at91_ide_info *info, const struct ata_timing *ata)
+{
+        int read_cycle, write_cycle, active, recover;
+        int nrd_setup, nrd_pulse, nrd_recover;
+        int nwe_setup, nwe_pulse;
+
+        int ncs_write_setup, ncs_write_pulse;
+        int ncs_read_setup, ncs_read_pulse;
+
+        unsigned int mck_hz;
+        struct clk *mck;
+
+        read_cycle  = ata->cyc8b;
+        nrd_setup   = ata->setup;
+        nrd_pulse   = ata->act8b;
+        nrd_recover = ata->rec8b;
+
+        mck = clk_get(NULL, "mck");
+        BUG_ON(IS_ERR(mck));
+        mck_hz = clk_get_rate(mck);
+
+        read_cycle  = calc_mck_cycles(read_cycle, mck_hz);
+        nrd_setup   = calc_mck_cycles(nrd_setup, mck_hz);
+        nrd_pulse   = calc_mck_cycles(nrd_pulse, mck_hz);
+        nrd_recover = calc_mck_cycles(nrd_recover, mck_hz);
+
+        clk_put(mck);
+
+        active  = nrd_setup + nrd_pulse;
+        recover = read_cycle - active;
+
+        /* Need at least two cycles recovery */
+        if (recover < 2)
+                read_cycle = active + 2;
+
+        /* (CS0, CS1, DIR, OE) <= (CFCE1, CFCE2, CFRNW, NCSX) timings */
+        ncs_read_setup = 1;
+        ncs_read_pulse = read_cycle - 2;
+
+        /* Write timings same as read timings */
+        write_cycle = read_cycle;
+        nwe_setup = nrd_setup;
+        nwe_pulse = nrd_pulse;
+        ncs_write_setup = ncs_read_setup;
+        ncs_write_pulse = ncs_read_pulse;
+
+        dev_dbg(dev, "ATA timings: nrd_setup = %d nrd_pulse = %d nrd_cycle = %d\n",
+                        nrd_setup, nrd_pulse, read_cycle);
+        dev_dbg(dev, "ATA timings: nwe_setup = %d nwe_pulse = %d nwe_cycle = %d\n",
+                        nwe_setup, nwe_pulse, write_cycle);
+        dev_dbg(dev, "ATA timings: ncs_read_setup = %d ncs_read_pulse = %d\n",
+                        ncs_read_setup, ncs_read_pulse);
+        dev_dbg(dev, "ATA timings: ncs_write_setup = %d ncs_write_pulse = %d\n",
+                        ncs_write_setup, ncs_write_pulse);
+
+        at91_sys_write(AT91_SMC_SETUP(info->cs),
+                        AT91_SMC_NWESETUP_(nwe_setup) |
+                        AT91_SMC_NRDSETUP_(nrd_setup) |
+                        AT91_SMC_NCS_WRSETUP_(ncs_write_setup) |
+                        AT91_SMC_NCS_RDSETUP_(ncs_read_setup));
+
+        at91_sys_write(AT91_SMC_PULSE(info->cs),
+                        AT91_SMC_NWEPULSE_(nwe_pulse) |
+                        AT91_SMC_NRDPULSE_(nrd_pulse) |
+                        AT91_SMC_NCS_WRPULSE_(ncs_write_pulse) |
+                        AT91_SMC_NCS_RDPULSE_(ncs_read_pulse));
+
+        at91_sys_write(AT91_SMC_CYCLE(info->cs),
+                        AT91_SMC_NWECYCLE_(write_cycle) |
+                        AT91_SMC_NRDCYCLE_(read_cycle));
+
+        return;
+}
+
+static void pata_at91_set_piomode(struct ata_port *ap, struct ata_device *adev)
+{
+        struct at91_ide_info *info = ap->host->private_data;
+        struct ata_timing timing;
+        int ret;
+
+        /* Compute ATA timing and set it to SMC */
+        ret = ata_timing_compute(adev, adev->pio_mode, &timing, 1000, 0);
+        if (ret) {
+                dev_warn(ap->dev, "Failed to compute ATA timing %d, \
+                                set PIO_0 timing\n", ret);
+                set_smc_timing(ap->dev, info, &initial_timing);
+        } else {
+                set_smc_timing(ap->dev, info, &timing);
+        }
+
+        /* Setup SMC mode */
+        set_smc_mode(info);
+
+        return;
+}
+
+static unsigned int pata_at91_data_xfer_noirq(struct ata_device *dev,
+                unsigned char *buf, unsigned int buflen, int rw)
+{
+        struct at91_ide_info *info = dev->link->ap->host->private_data;
+        unsigned int consumed;
+        unsigned long flags;
+        unsigned int mode;
+
+        local_irq_save(flags);
+        mode = at91_sys_read(AT91_SMC_MODE(info->cs));
+
+        /* set 16bit mode before writing data */
+        at91_sys_write(AT91_SMC_MODE(info->cs),
+                        (mode & ~AT91_SMC_DBW) | AT91_SMC_DBW_16);
+
+        consumed = ata_sff_data_xfer(dev, buf, buflen, rw);
+
+        /* restore 8bit mode after data is written */
+        at91_sys_write(AT91_SMC_MODE(info->cs),
+                        (mode & ~AT91_SMC_DBW) | AT91_SMC_DBW_8);
+
+        local_irq_restore(flags);
+        return consumed;
+}
+
+static struct scsi_host_template pata_at91_sht = {
+        ATA_PIO_SHT(DRV_NAME),
+};
+
+static struct ata_port_operations pata_at91_port_ops = {
+        .inherits       = &ata_sff_port_ops,
+
+        .sff_data_xfer  = pata_at91_data_xfer_noirq,
+        .set_piomode    = pata_at91_set_piomode,
+        .cable_detect   = ata_cable_40wire,
+        .port_start     = ATA_OP_NULL,
+};
+
+static int __devinit pata_at91_probe(struct platform_device *pdev)
+{
+        struct at91_cf_data *board = pdev->dev.platform_data;
+        struct device *dev = &pdev->dev;
+        struct at91_ide_info *info;
+        struct resource *mem_res;
+        struct ata_host *host;
+        struct ata_port *ap;
+        int irq_flags = 0;
+        int irq = 0;
+        int ret;
+
+        /*  get platform resources: IO/CTL memories and irq/rst pins */
+
+        if (pdev->num_resources != 1) {
+                dev_err(&pdev->dev, "invalid number of resources\n");
+                return -EINVAL;
+        }
+
+        mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+        if (!mem_res) {
+                dev_err(dev, "failed to get mem resource\n");
+                return -EINVAL;
+        }
+
+        irq = board->irq_pin;
+
+        /* init ata host */
+
+        host = ata_host_alloc(dev, 1);
+
+        if (!host)
+                return -ENOMEM;
+
+        ap = host->ports[0];
+        ap->ops = &pata_at91_port_ops;
+        ap->flags |= ATA_FLAG_SLAVE_POSS;
+        ap->pio_mask = ATA_PIO4;
+
+        if (!irq) {
+                ap->flags |= ATA_FLAG_PIO_POLLING;
+                ata_port_desc(ap, "no IRQ, using PIO polling");
+        }
+
+        info = kzalloc(sizeof(*info), GFP_KERNEL);
+
+        if (!info) {
+                dev_err(dev, "failed to allocate memory for private data\n");
+                return -ENOMEM;
+        }
+
+        info->cs    = board->chipselect;
+        info->mode  = AT91_SMC_READMODE | AT91_SMC_WRITEMODE |
+                AT91_SMC_EXNWMODE_READY | AT91_SMC_BAT_SELECT |
+                AT91_SMC_DBW_8 | AT91_SMC_TDF_(0);
+
+        info->ide_addr = devm_ioremap(dev,
+                        mem_res->start + CF_IDE_OFFSET, CF_IDE_RES_SIZE);
+
+        if (!info->ide_addr) {
+                dev_err(dev, "failed to map IO base\n");
+                ret = -ENOMEM;
+                goto err_ide_ioremap;
+        }
+
+        info->alt_addr = devm_ioremap(dev,
+                        mem_res->start + CF_ALT_IDE_OFFSET, CF_IDE_RES_SIZE);
+
+        if (!info->alt_addr) {
+                dev_err(dev, "failed to map CTL base\n");
+                ret = -ENOMEM;
+                goto err_alt_ioremap;
+        }
+
+        ap->ioaddr.cmd_addr = info->ide_addr;
+        ap->ioaddr.ctl_addr = info->alt_addr + 0x06;
+        ap->ioaddr.altstatus_addr = ap->ioaddr.ctl_addr;
+
+        ata_sff_std_ports(&ap->ioaddr);
+
+        ata_port_desc(ap, "mmio cmd 0x%llx ctl 0x%llx",
+                        (unsigned long long)mem_res->start + CF_IDE_OFFSET,
+                        (unsigned long long)mem_res->start + CF_ALT_IDE_OFFSET);
+
+        host->private_data = info;
+
+        return ata_host_activate(host, irq ? gpio_to_irq(irq) : 0,
+                        irq ? ata_sff_interrupt : NULL,
+                        irq_flags, &pata_at91_sht);
+
+err_alt_ioremap:
+        devm_iounmap(dev, info->ide_addr);
+
+err_ide_ioremap:
+        kfree(info);
+
+        return ret;
+}
+
+static int __devexit pata_at91_remove(struct platform_device *pdev)
+{
+        struct ata_host *host = dev_get_drvdata(&pdev->dev);
+        struct at91_ide_info *info = host->private_data;
+        struct device *dev = &pdev->dev;
+
+        if (!host)
+                return 0;
+
+        ata_host_detach(host);
+
+        if (!info)
+                return 0;
+
+        devm_iounmap(dev, info->ide_addr);
+        devm_iounmap(dev, info->alt_addr);
+
+        kfree(info);
+        return 0;
+}
+
+static struct platform_driver pata_at91_driver = {
+        .probe          = pata_at91_probe,
+        .remove         = __devexit_p(pata_at91_remove),
+        .driver         = {
+                .name           = DRV_NAME,
+                .owner          = THIS_MODULE,
+        },
+};
+
+static int __init pata_at91_init(void)
+{
+        return platform_driver_register(&pata_at91_driver);
+}
+
+static void __exit pata_at91_exit(void)
+{
+        platform_driver_unregister(&pata_at91_driver);
+}
+
+
+module_init(pata_at91_init);
+module_exit(pata_at91_exit);
+
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Driver for CF in True IDE mode on AT91SAM9260 SoC");
+MODULE_AUTHOR("Matyukevich Sergey");
+MODULE_VERSION(DRV_VERSION);
+
diff --git a/drivers/ata/sata_fsl.c b/drivers/ata/sata_fsl.c
index 36b8629203be..94eaa432c40a 100644
--- a/drivers/ata/sata_fsl.c
+++ b/drivers/ata/sata_fsl.c
@@ -1378,6 +1378,37 @@ static int sata_fsl_remove(struct of_device *ofdev)
         return 0;
 }
 
+#ifdef CONFIG_PM
+static int sata_fsl_suspend(struct of_device *op, pm_message_t state)
+{
+        struct ata_host *host = dev_get_drvdata(&op->dev);
+        return ata_host_suspend(host, state);
+}
+
+static int sata_fsl_resume(struct of_device *op)
+{
+        struct ata_host *host = dev_get_drvdata(&op->dev);
+        struct sata_fsl_host_priv *host_priv = host->private_data;
+        int ret;
+        void __iomem *hcr_base = host_priv->hcr_base;
+        struct ata_port *ap = host->ports[0];
+        struct sata_fsl_port_priv *pp = ap->private_data;
+
+        ret = sata_fsl_init_controller(host);
+        if (ret) {
+                dev_printk(KERN_ERR, &op->dev,
+                        "Error initialize hardware\n");
+                return ret;
+        }
+
+        /* Recovery the CHBA register in host controller cmd register set */
+        iowrite32(pp->cmdslot_paddr & 0xffffffff, hcr_base + CHBA);
+
+        ata_host_resume(host);
+        return 0;
+}
+#endif
+
 static struct of_device_id fsl_sata_match[] = {
         {
                 .compatible = "fsl,pq-sata",
@@ -1392,6 +1423,10 @@ static struct of_platform_driver fsl_sata_driver = {
         .match_table    = fsl_sata_match,
         .probe          = sata_fsl_probe,
         .remove         = sata_fsl_remove,
+#ifdef CONFIG_PM
+        .suspend        = sata_fsl_suspend,
+        .resume         = sata_fsl_resume,
+#endif
 };
 
 static int __init sata_fsl_init(void)
diff --git a/drivers/char/bfin_jtag_comm.c b/drivers/char/bfin_jtag_comm.c
index 44c113d56045..1d7c34c73b20 100644
--- a/drivers/char/bfin_jtag_comm.c
+++ b/drivers/char/bfin_jtag_comm.c
@@ -8,6 +8,10 @@
  * Licensed under the GPL-2 or later.
  */
 
+#define DRV_NAME "bfin-jtag-comm"
+#define DEV_NAME "ttyBFJC"
+#define pr_fmt(fmt) DRV_NAME ": " fmt
+
 #include <linux/circ_buf.h>
 #include <linux/console.h>
 #include <linux/delay.h>
@@ -22,18 +26,14 @@
 #include <linux/tty_flip.h>
 #include <asm/atomic.h>
 
+#define pr_init(fmt, args...) ({ static const __initconst char __fmt[] = fmt; printk(__fmt, ## args); })
+
 /* See the Debug/Emulation chapter in the HRM */
 #define EMUDOF   0x00000001     /* EMUDAT_OUT full & valid */
 #define EMUDIF   0x00000002     /* EMUDAT_IN full & valid */
 #define EMUDOOVF 0x00000004     /* EMUDAT_OUT overflow */
 #define EMUDIOVF 0x00000008     /* EMUDAT_IN overflow */
 
-#define DRV_NAME "bfin-jtag-comm"
-#define DEV_NAME "ttyBFJC"
-
-#define pr_init(fmt, args...) ({ static const __initdata char __fmt[] = fmt; printk(__fmt, ## args); })
-#define debug(fmt, args...) pr_debug(DRV_NAME ": " fmt, ## args)
-
 static inline uint32_t bfin_write_emudat(uint32_t emudat)
 {
         __asm__ __volatile__("emudat = %0;" : : "d"(emudat));
@@ -74,7 +74,7 @@ bfin_jc_emudat_manager(void *arg)
         while (!kthread_should_stop()) {
                 /* no one left to give data to, so sleep */
                 if (bfin_jc_tty == NULL && circ_empty(&bfin_jc_write_buf)) {
-                        debug("waiting for readers\n");
+                        pr_debug("waiting for readers\n");
                         __set_current_state(TASK_UNINTERRUPTIBLE);
                         schedule();
                         __set_current_state(TASK_RUNNING);
@@ -82,7 +82,7 @@ bfin_jc_emudat_manager(void *arg)
 
                 /* no data available, so just chill */
                 if (!(bfin_read_DBGSTAT() & EMUDIF) && circ_empty(&bfin_jc_write_buf)) {
-                        debug("waiting for data (in_len = %i) (circ: %i %i)\n",
+                        pr_debug("waiting for data (in_len = %i) (circ: %i %i)\n",
                                 inbound_len, bfin_jc_write_buf.tail, bfin_jc_write_buf.head);
                         if (inbound_len)
                                 schedule();
@@ -99,11 +99,11 @@ bfin_jc_emudat_manager(void *arg)
                         if (tty != NULL) {
                                 uint32_t emudat = bfin_read_emudat();
                                 if (inbound_len == 0) {
-                                        debug("incoming length: 0x%08x\n", emudat);
+                                        pr_debug("incoming length: 0x%08x\n", emudat);
                                         inbound_len = emudat;
                                 } else {
                                         size_t num_chars = (4 <= inbound_len ? 4 : inbound_len);
-                                        debug("  incoming data: 0x%08x (pushing %zu)\n", emudat, num_chars);
+                                        pr_debug("  incoming data: 0x%08x (pushing %zu)\n", emudat, num_chars);
                                         inbound_len -= num_chars;
                                         tty_insert_flip_string(tty, (unsigned char *)&emudat, num_chars);
                                         tty_flip_buffer_push(tty);
@@ -117,7 +117,7 @@ bfin_jc_emudat_manager(void *arg)
                         if (outbound_len == 0) {
                                 outbound_len = circ_cnt(&bfin_jc_write_buf);
                                 bfin_write_emudat(outbound_len);
-                                debug("outgoing length: 0x%08x\n", outbound_len);
+                                pr_debug("outgoing length: 0x%08x\n", outbound_len);
                         } else {
                                 struct tty_struct *tty;
                                 int tail = bfin_jc_write_buf.tail;
@@ -136,7 +136,7 @@ bfin_jc_emudat_manager(void *arg)
                                 if (tty)
                                         tty_wakeup(tty);
                                 mutex_unlock(&bfin_jc_tty_mutex);
-                                debug("  outgoing data: 0x%08x (pushing %zu)\n", emudat, ate);
+                                pr_debug("  outgoing data: 0x%08x (pushing %zu)\n", emudat, ate);
                         }
                 }
         }
@@ -149,7 +149,7 @@ static int
 bfin_jc_open(struct tty_struct *tty, struct file *filp)
 {
         mutex_lock(&bfin_jc_tty_mutex);
-        debug("open %lu\n", bfin_jc_count);
+        pr_debug("open %lu\n", bfin_jc_count);
         ++bfin_jc_count;
         bfin_jc_tty = tty;
         wake_up_process(bfin_jc_kthread);
@@ -161,7 +161,7 @@ static void
 bfin_jc_close(struct tty_struct *tty, struct file *filp)
 {
         mutex_lock(&bfin_jc_tty_mutex);
-        debug("close %lu\n", bfin_jc_count);
+        pr_debug("close %lu\n", bfin_jc_count);
         if (--bfin_jc_count == 0)
                 bfin_jc_tty = NULL;
         wake_up_process(bfin_jc_kthread);
@@ -174,7 +174,7 @@ bfin_jc_circ_write(const unsigned char *buf, int count)
 {
         int i;
         count = min(count, circ_free(&bfin_jc_write_buf));
-        debug("going to write chunk of %i bytes\n", count);
+        pr_debug("going to write chunk of %i bytes\n", count);
         for (i = 0; i < count; ++i)
                 circ_byte(&bfin_jc_write_buf, bfin_jc_write_buf.head + i) = buf[i];
         bfin_jc_write_buf.head += i;
diff --git a/drivers/char/moxa.c b/drivers/char/moxa.c
index 6799588b0099..65b6ff2442c6 100644
--- a/drivers/char/moxa.c
+++ b/drivers/char/moxa.c
@@ -1189,11 +1189,6 @@ static int moxa_open(struct tty_struct *tty, struct file *filp)
                 return -ENODEV;
         }
 
-        if (port % MAX_PORTS_PER_BOARD >= brd->numPorts) {
-                retval = -ENODEV;
-                goto out_unlock;
-        }
-
         ch = &brd->ports[port % MAX_PORTS_PER_BOARD];
         ch->port.count++;
         tty->driver_data = ch;
@@ -1218,8 +1213,8 @@ static int moxa_open(struct tty_struct *tty, struct file *filp)
                                 moxa_close_port(tty);
         } else
                 ch->port.flags |= ASYNC_NORMAL_ACTIVE;
-out_unlock:
         mutex_unlock(&moxa_openlock);
+
         return retval;
 }
 
diff --git a/drivers/char/n_hdlc.c b/drivers/char/n_hdlc.c
index 461ece591a5b..1c43c8cdee25 100644
--- a/drivers/char/n_hdlc.c
+++ b/drivers/char/n_hdlc.c
@@ -10,7 +10,6 @@
  *      Paul Mackerras <Paul.Mackerras@cs.anu.edu.au>
  *
  * Original release 01/11/99
- * $Id: n_hdlc.c,v 4.8 2003/05/06 21:18:51 paulkf Exp $
  *
  * This code is released under the GNU General Public License (GPL)
  *
@@ -79,7 +78,6 @@
  */
 
 #define HDLC_MAGIC 0x239e
-#define HDLC_VERSION "$Revision: 4.8 $"
 
 #include <linux/module.h>
 #include <linux/init.h>
@@ -114,7 +112,7 @@
 #define MAX_HDLC_FRAME_SIZE 65535 
 #define DEFAULT_RX_BUF_COUNT 10
 #define MAX_RX_BUF_COUNT 60
-#define DEFAULT_TX_BUF_COUNT 1
+#define DEFAULT_TX_BUF_COUNT 3
 
 struct n_hdlc_buf {
         struct n_hdlc_buf *link;
@@ -199,6 +197,31 @@ static void n_hdlc_tty_wakeup(struct tty_struct *tty);
 #define tty2n_hdlc(tty) ((struct n_hdlc *) ((tty)->disc_data))
 #define n_hdlc2tty(n_hdlc)      ((n_hdlc)->tty)
 
+static void flush_rx_queue(struct tty_struct *tty)
+{
+        struct n_hdlc *n_hdlc = tty2n_hdlc(tty);
+        struct n_hdlc_buf *buf;
+
+        while ((buf = n_hdlc_buf_get(&n_hdlc->rx_buf_list)))
+                n_hdlc_buf_put(&n_hdlc->rx_free_buf_list, buf);
+}
+
+static void flush_tx_queue(struct tty_struct *tty)
+{
+        struct n_hdlc *n_hdlc = tty2n_hdlc(tty);
+        struct n_hdlc_buf *buf;
+        unsigned long flags;
+
+        while ((buf = n_hdlc_buf_get(&n_hdlc->tx_buf_list)))
+                n_hdlc_buf_put(&n_hdlc->tx_free_buf_list, buf);
+        spin_lock_irqsave(&n_hdlc->tx_buf_list.spinlock, flags);
+        if (n_hdlc->tbuf) {
+                n_hdlc_buf_put(&n_hdlc->tx_free_buf_list, n_hdlc->tbuf);
+                n_hdlc->tbuf = NULL;
+        }
+        spin_unlock_irqrestore(&n_hdlc->tx_buf_list.spinlock, flags);
+}
+
 static struct tty_ldisc_ops n_hdlc_ldisc = {
         .owner          = THIS_MODULE,
         .magic          = TTY_LDISC_MAGIC,
@@ -211,6 +234,7 @@ static struct tty_ldisc_ops n_hdlc_ldisc = {
         .poll           = n_hdlc_tty_poll,
         .receive_buf    = n_hdlc_tty_receive,
         .write_wakeup   = n_hdlc_tty_wakeup,
+        .flush_buffer   = flush_rx_queue,
 };
 
 /**
@@ -341,10 +365,7 @@ static int n_hdlc_tty_open (struct tty_struct *tty)
         set_bit(TTY_NO_WRITE_SPLIT,&tty->flags);
 #endif
         
-        /* Flush any pending characters in the driver and discipline. */
-        if (tty->ldisc->ops->flush_buffer)
-                tty->ldisc->ops->flush_buffer(tty);
-
+        /* flush receive data from driver */
         tty_driver_flush_buffer(tty);
                 
         if (debuglevel >= DEBUG_LEVEL_INFO)     
@@ -763,6 +784,14 @@ static int n_hdlc_tty_ioctl(struct tty_struct *tty, struct file *file,
                 error = put_user(count, (int __user *)arg);
                 break;
 
+        case TCFLSH:
+                switch (arg) {
+                case TCIOFLUSH:
+                case TCOFLUSH:
+                        flush_tx_queue(tty);
+                }
+                /* fall through to default */
+
         default:
                 error = n_tty_ioctl_helper(tty, file, cmd, arg);
                 break;
@@ -919,8 +948,7 @@ static struct n_hdlc_buf* n_hdlc_buf_get(struct n_hdlc_buf_list *list)
 }       /* end of n_hdlc_buf_get() */
 
 static char hdlc_banner[] __initdata =
-        KERN_INFO "HDLC line discipline: version " HDLC_VERSION
-        ", maxframe=%u\n";
+        KERN_INFO "HDLC line discipline maxframe=%u\n";
 static char hdlc_register_ok[] __initdata =
         KERN_INFO "N_HDLC line discipline registered.\n";
 static char hdlc_register_fail[] __initdata =
diff --git a/drivers/char/n_r3964.c b/drivers/char/n_r3964.c
index d2e93e343226..2e99158ebb8a 100644
--- a/drivers/char/n_r3964.c
+++ b/drivers/char/n_r3964.c
@@ -1062,7 +1062,7 @@ static ssize_t r3964_read(struct tty_struct *tty, struct file *file,
         struct r3964_client_info *pClient;
         struct r3964_message *pMsg;
         struct r3964_client_message theMsg;
-        int count;
+        int ret;
 
         TRACE_L("read()");
 
@@ -1074,8 +1074,8 @@ static ssize_t r3964_read(struct tty_struct *tty, struct file *file,
                 if (pMsg == NULL) {
                         /* no messages available. */
                         if (file->f_flags & O_NONBLOCK) {
-                                unlock_kernel();
-                                return -EAGAIN;
+                                ret = -EAGAIN;
+                                goto unlock;
                         }
                         /* block until there is a message: */
                         wait_event_interruptible(pInfo->read_wait,
@@ -1085,29 +1085,31 @@ static ssize_t r3964_read(struct tty_struct *tty, struct file *file,
                 /* If we still haven't got a message, we must have been signalled */
 
                 if (!pMsg) {
-                        unlock_kernel();
-                        return -EINTR;
+                        ret = -EINTR;
+                        goto unlock;
                 }
 
                 /* deliver msg to client process: */
                 theMsg.msg_id = pMsg->msg_id;
                 theMsg.arg = pMsg->arg;
                 theMsg.error_code = pMsg->error_code;
-                count = sizeof(struct r3964_client_message);
+                ret = sizeof(struct r3964_client_message);
 
                 kfree(pMsg);
                 TRACE_M("r3964_read - msg kfree %p", pMsg);
 
-                if (copy_to_user(buf, &theMsg, count)) {
-                        unlock_kernel();
-                        return -EFAULT;
+                if (copy_to_user(buf, &theMsg, ret)) {
+                        ret = -EFAULT;
+                        goto unlock;
                 }
 
-                TRACE_PS("read - return %d", count);
-                return count;
+                TRACE_PS("read - return %d", ret);
+                goto unlock;
         }
+        ret = -EPERM;
+unlock:
         unlock_kernel();
-        return -EPERM;
+        return ret;
 }
 
 static ssize_t r3964_write(struct tty_struct *tty, struct file *file,
diff --git a/drivers/char/pcmcia/cm4000_cs.c b/drivers/char/pcmcia/cm4000_cs.c
index dbb912574569..881934c068c8 100644
--- a/drivers/char/pcmcia/cm4000_cs.c
+++ b/drivers/char/pcmcia/cm4000_cs.c
@@ -1575,7 +1575,8 @@ static long cmm_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
                 clear_bit(LOCK_IO, &dev->flags);
                 wake_up_interruptible(&dev->ioq);
 
-                return 0;
+                rc = 0;
+                break;
         case CM_IOCSPTS:
                 {
                         struct ptsreq krnptsreq;
diff --git a/drivers/char/vt_ioctl.c b/drivers/char/vt_ioctl.c
index e6ce632a393e..7539bed0f7e0 100644
--- a/drivers/char/vt_ioctl.c
+++ b/drivers/char/vt_ioctl.c
@@ -396,7 +396,8 @@ int vt_ioctl(struct tty_struct *tty, struct file * file,
         kbd = kbd_table + console;
         switch (cmd) {
         case TIOCLINUX:
-                return tioclinux(tty, arg);
+                ret = tioclinux(tty, arg);
+                break;
         case KIOCSOUND:
                 if (!perm)
                         goto eperm;
diff --git a/drivers/crypto/padlock-aes.c b/drivers/crypto/padlock-aes.c
index 87f92c39b5f0..a9952b1236b0 100644
--- a/drivers/crypto/padlock-aes.c
+++ b/drivers/crypto/padlock-aes.c
@@ -18,9 +18,22 @@
 #include <linux/percpu.h>
 #include <linux/smp.h>
 #include <asm/byteorder.h>
+#include <asm/processor.h>
 #include <asm/i387.h>
 #include "padlock.h"
 
+/*
+ * Number of data blocks actually fetched for each xcrypt insn.
+ * Processors with prefetch errata will fetch extra blocks.
+ */
+static unsigned int ecb_fetch_blocks = 2;
+#define MAX_ECB_FETCH_BLOCKS (8)
+#define ecb_fetch_bytes (ecb_fetch_blocks * AES_BLOCK_SIZE)
+
+static unsigned int cbc_fetch_blocks = 1;
+#define MAX_CBC_FETCH_BLOCKS (4)
+#define cbc_fetch_bytes (cbc_fetch_blocks * AES_BLOCK_SIZE)
+
 /* Control word. */
 struct cword {
         unsigned int __attribute__ ((__packed__))
@@ -172,73 +185,111 @@ static inline void padlock_store_cword(struct cword *cword)
  * should be used only inside the irq_ts_save/restore() context
  */
 
-static inline void padlock_xcrypt(const u8 *input, u8 *output, void *key,
-                                  struct cword *control_word)
+static inline void rep_xcrypt_ecb(const u8 *input, u8 *output, void *key,
+                                  struct cword *control_word, int count)
 {
         asm volatile (".byte 0xf3,0x0f,0xa7,0xc8"       /* rep xcryptecb */
                       : "+S"(input), "+D"(output)
-                      : "d"(control_word), "b"(key), "c"(1));
+                      : "d"(control_word), "b"(key), "c"(count));
+}
+
+static inline u8 *rep_xcrypt_cbc(const u8 *input, u8 *output, void *key,
+                                 u8 *iv, struct cword *control_word, int count)
+{
+        asm volatile (".byte 0xf3,0x0f,0xa7,0xd0"       /* rep xcryptcbc */
+                      : "+S" (input), "+D" (output), "+a" (iv)
+                      : "d" (control_word), "b" (key), "c" (count));
+        return iv;
 }
 
-static void aes_crypt_copy(const u8 *in, u8 *out, u32 *key, struct cword *cword)
+static void ecb_crypt_copy(const u8 *in, u8 *out, u32 *key,
+                           struct cword *cword, int count)
 {
-        u8 buf[AES_BLOCK_SIZE * 2 + PADLOCK_ALIGNMENT - 1];
+        /*
+         * Padlock prefetches extra data so we must provide mapped input buffers.
+         * Assume there are at least 16 bytes of stack already in use.
+         */
+        u8 buf[AES_BLOCK_SIZE * (MAX_ECB_FETCH_BLOCKS - 1) + PADLOCK_ALIGNMENT - 1];
+        u8 *tmp = PTR_ALIGN(&buf[0], PADLOCK_ALIGNMENT);
+
+        memcpy(tmp, in, count * AES_BLOCK_SIZE);
+        rep_xcrypt_ecb(tmp, out, key, cword, count);
+}
+
+static u8 *cbc_crypt_copy(const u8 *in, u8 *out, u32 *key,
+                           u8 *iv, struct cword *cword, int count)
+{
+        /*
+         * Padlock prefetches extra data so we must provide mapped input buffers.
+         * Assume there are at least 16 bytes of stack already in use.
+         */
+        u8 buf[AES_BLOCK_SIZE * (MAX_CBC_FETCH_BLOCKS - 1) + PADLOCK_ALIGNMENT - 1];
         u8 *tmp = PTR_ALIGN(&buf[0], PADLOCK_ALIGNMENT);
 
-        memcpy(tmp, in, AES_BLOCK_SIZE);
-        padlock_xcrypt(tmp, out, key, cword);
+        memcpy(tmp, in, count * AES_BLOCK_SIZE);
+        return rep_xcrypt_cbc(tmp, out, key, iv, cword, count);
 }
 
-static inline void aes_crypt(const u8 *in, u8 *out, u32 *key,
-                             struct cword *cword)
+static inline void ecb_crypt(const u8 *in, u8 *out, u32 *key,
+                             struct cword *cword, int count)
 {
-        /* padlock_xcrypt requires at least two blocks of data. */
-        if (unlikely(!(((unsigned long)in ^ (PAGE_SIZE - AES_BLOCK_SIZE)) &
-                       (PAGE_SIZE - 1)))) {
-                aes_crypt_copy(in, out, key, cword);
+        /* Padlock in ECB mode fetches at least ecb_fetch_bytes of data.
+         * We could avoid some copying here but it's probably not worth it.
+         */
+        if (unlikely(((unsigned long)in & PAGE_SIZE) + ecb_fetch_bytes > PAGE_SIZE)) {
+                ecb_crypt_copy(in, out, key, cword, count);
                 return;
         }
 
-        padlock_xcrypt(in, out, key, cword);
+        rep_xcrypt_ecb(in, out, key, cword, count);
+}
+
+static inline u8 *cbc_crypt(const u8 *in, u8 *out, u32 *key,
+                            u8 *iv, struct cword *cword, int count)
+{
+        /* Padlock in CBC mode fetches at least cbc_fetch_bytes of data. */
+        if (unlikely(((unsigned long)in & PAGE_SIZE) + cbc_fetch_bytes > PAGE_SIZE))
+                return cbc_crypt_copy(in, out, key, iv, cword, count);
+
+        return rep_xcrypt_cbc(in, out, key, iv, cword, count);
 }
 
 static inline void padlock_xcrypt_ecb(const u8 *input, u8 *output, void *key,
                                       void *control_word, u32 count)
 {
-        if (count == 1) {
-                aes_crypt(input, output, key, control_word);
+        u32 initial = count & (ecb_fetch_blocks - 1);
+
+        if (count < ecb_fetch_blocks) {
+                ecb_crypt(input, output, key, control_word, count);
                 return;
         }
 
-        asm volatile ("test $1, %%cl;"
-                      "je 1f;"
-#ifndef CONFIG_X86_64
-                      "lea -1(%%ecx), %%eax;"
-                      "mov $1, %%ecx;"
-#else
-                      "lea -1(%%rcx), %%rax;"
-                      "mov $1, %%rcx;"
-#endif
-                      ".byte 0xf3,0x0f,0xa7,0xc8;"      /* rep xcryptecb */
-#ifndef CONFIG_X86_64
-                      "mov %%eax, %%ecx;"
-#else
-                      "mov %%rax, %%rcx;"
-#endif
-                      "1:"
-                      ".byte 0xf3,0x0f,0xa7,0xc8"       /* rep xcryptecb */
+        if (initial)
+                asm volatile (".byte 0xf3,0x0f,0xa7,0xc8"       /* rep xcryptecb */
+                              : "+S"(input), "+D"(output)
+                              : "d"(control_word), "b"(key), "c"(initial));
+
+        asm volatile (".byte 0xf3,0x0f,0xa7,0xc8"       /* rep xcryptecb */
                       : "+S"(input), "+D"(output)
-                      : "d"(control_word), "b"(key), "c"(count)
-                      : "ax");
+                      : "d"(control_word), "b"(key), "c"(count - initial));
 }
 
 static inline u8 *padlock_xcrypt_cbc(const u8 *input, u8 *output, void *key,
                                      u8 *iv, void *control_word, u32 count)
 {
-        /* rep xcryptcbc */
-        asm volatile (".byte 0xf3,0x0f,0xa7,0xd0"
+        u32 initial = count & (cbc_fetch_blocks - 1);
+
+        if (count < cbc_fetch_blocks)
+                return cbc_crypt(input, output, key, iv, control_word, count);
+
+        if (initial)
+                asm volatile (".byte 0xf3,0x0f,0xa7,0xd0"       /* rep xcryptcbc */
+                              : "+S" (input), "+D" (output), "+a" (iv)
+                              : "d" (control_word), "b" (key), "c" (count));
+
+        asm volatile (".byte 0xf3,0x0f,0xa7,0xd0"       /* rep xcryptcbc */
                       : "+S" (input), "+D" (output), "+a" (iv)
-                      : "d" (control_word), "b" (key), "c" (count));
+                      : "d" (control_word), "b" (key), "c" (count-initial));
         return iv;
 }
 
@@ -249,7 +300,7 @@ static void aes_encrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
 
         padlock_reset_key(&ctx->cword.encrypt);
         ts_state = irq_ts_save();
-        aes_crypt(in, out, ctx->E, &ctx->cword.encrypt);
+        ecb_crypt(in, out, ctx->E, &ctx->cword.encrypt, 1);
         irq_ts_restore(ts_state);
         padlock_store_cword(&ctx->cword.encrypt);
 }
@@ -261,7 +312,7 @@ static void aes_decrypt(struct crypto_tfm *tfm, u8 *out, const u8 *in)
 
         padlock_reset_key(&ctx->cword.encrypt);
         ts_state = irq_ts_save();
-        aes_crypt(in, out, ctx->D, &ctx->cword.decrypt);
+        ecb_crypt(in, out, ctx->D, &ctx->cword.decrypt, 1);
         irq_ts_restore(ts_state);
         padlock_store_cword(&ctx->cword.encrypt);
 }
@@ -454,6 +505,7 @@ static struct crypto_alg cbc_aes_alg = {
 static int __init padlock_init(void)
 {
         int ret;
+        struct cpuinfo_x86 *c = &cpu_data(0);
 
         if (!cpu_has_xcrypt) {
                 printk(KERN_NOTICE PFX "VIA PadLock not detected.\n");
@@ -476,6 +528,12 @@ static int __init padlock_init(void)
 
         printk(KERN_NOTICE PFX "Using VIA PadLock ACE for AES algorithm.\n");
 
+        if (c->x86 == 6 && c->x86_model == 15 && c->x86_mask == 2) {
+                ecb_fetch_blocks = MAX_ECB_FETCH_BLOCKS;
+                cbc_fetch_blocks = MAX_CBC_FETCH_BLOCKS;
+                printk(KERN_NOTICE PFX "VIA Nano stepping 2 detected: enabling workaround.\n");
+        }
+
 out:
         return ret;
 
diff --git a/drivers/firewire/Kconfig b/drivers/firewire/Kconfig
index 450902438208..13efcd362072 100644
--- a/drivers/firewire/Kconfig
+++ b/drivers/firewire/Kconfig
@@ -1,28 +1,29 @@
-comment "A new alternative FireWire stack is available with EXPERIMENTAL=y"
-        depends on EXPERIMENTAL=n
-
-comment "Enable only one of the two stacks, unless you know what you are doing"
-        depends on EXPERIMENTAL
+comment "You can enable one or both FireWire driver stacks."
+comment "See the help texts for more information."
 
 config FIREWIRE
-        tristate "New FireWire stack, EXPERIMENTAL"
-        depends on EXPERIMENTAL
+        tristate "FireWire driver stack"
         select CRC_ITU_T
         help
-          This is the "Juju" FireWire stack, a new alternative implementation
-          designed for robustness and simplicity.  You can build either this
-          stack, or the old stack (the ieee1394 driver, ohci1394 etc.) or both.
-          Please read http://ieee1394.wiki.kernel.org/index.php/Juju_Migration
-          before you enable the new stack.
+          This is the new-generation IEEE 1394 (FireWire) driver stack
+          a.k.a. Juju, a new implementation designed for robustness and
+          simplicity.
+          See http://ieee1394.wiki.kernel.org/index.php/Juju_Migration
+          for information about migration from the older Linux 1394 stack
+          to the new driver stack.
 
           To compile this driver as a module, say M here: the module will be
           called firewire-core.
 
           This module functionally replaces ieee1394, raw1394, and video1394.
           To access it from application programs, you generally need at least
-          libraw1394 version 2.  IIDC/DCAM applications also need libdc1394
-          version 2.  No libraries are required to access storage devices
-          through the firewire-sbp2 driver.
+          libraw1394 v2.  IIDC/DCAM applications need libdc1394 v2.
+          No libraries are required to access storage devices through the
+          firewire-sbp2 driver.
+
+          NOTE:
+          FireWire audio devices currently require the old drivers (ieee1394,
+          ohci1394, raw1394).
 
 config FIREWIRE_OHCI
         tristate "OHCI-1394 controllers"
@@ -37,11 +38,9 @@ config FIREWIRE_OHCI
           stack.
 
           NOTE:
-
-          You should only build either firewire-ohci or the old ohci1394 driver,
-          but not both.  If you nevertheless want to install both, you should
-          configure them only as modules and blacklist the driver(s) which you
-          don't want to have auto-loaded.  Add either
+          If you want to install firewire-ohci and ohci1394 together, you
+          should configure them only as modules and blacklist the driver(s)
+          which you don't want to have auto-loaded.  Add either
 
               blacklist firewire-ohci
           or
@@ -50,12 +49,7 @@ config FIREWIRE_OHCI
               blacklist dv1394
 
           to /etc/modprobe.conf or /etc/modprobe.d/* and update modprobe.conf
-          depending on your distribution.  The latter two modules should be
-          blacklisted together with ohci1394 because they depend on ohci1394.
-
-          If you have an old modprobe which doesn't implement the blacklist
-          directive, use "install modulename /bin/true" for the modules to be
-          blacklisted.
+          depending on your distribution.
 
 config FIREWIRE_OHCI_DEBUG
         bool
@@ -77,3 +71,17 @@ config FIREWIRE_SBP2
 
           You should also enable support for disks, CD-ROMs, etc. in the SCSI
           configuration section.
+
+config FIREWIRE_NET
+        tristate "IP networking over 1394 (EXPERIMENTAL)"
+        depends on FIREWIRE && INET && EXPERIMENTAL
+        help
+          This enables IPv4 over IEEE 1394, providing IP connectivity with
+          other implementations of RFC 2734 as found on several operating
+          systems.  Multicast support is currently limited.
+
+          NOTE, this driver is not stable yet!
+
+          To compile this driver as a module, say M here:  The module will be
+          called firewire-net.  It replaces eth1394 of the classic IEEE 1394
+          stack.
diff --git a/drivers/firewire/Makefile b/drivers/firewire/Makefile
index bc3b9bf822bf..a8f9bb6d9fdf 100644
--- a/drivers/firewire/Makefile
+++ b/drivers/firewire/Makefile
@@ -6,7 +6,9 @@ firewire-core-y += core-card.o core-cdev.o core-device.o \
                    core-iso.o core-topology.o core-transaction.o
 firewire-ohci-y += ohci.o
 firewire-sbp2-y += sbp2.o
+firewire-net-y  += net.o
 
-obj-$(CONFIG_FIREWIRE) += firewire-core.o
+obj-$(CONFIG_FIREWIRE)      += firewire-core.o
 obj-$(CONFIG_FIREWIRE_OHCI) += firewire-ohci.o
 obj-$(CONFIG_FIREWIRE_SBP2) += firewire-sbp2.o
+obj-$(CONFIG_FIREWIRE_NET)  += firewire-net.o
diff --git a/drivers/firewire/core-card.c b/drivers/firewire/core-card.c
index 4c1be64fdddd..543fccac81bb 100644
--- a/drivers/firewire/core-card.c
+++ b/drivers/firewire/core-card.c
@@ -176,6 +176,7 @@ int fw_core_add_descriptor(struct fw_descriptor *desc)
 
         return 0;
 }
+EXPORT_SYMBOL(fw_core_add_descriptor);
 
 void fw_core_remove_descriptor(struct fw_descriptor *desc)
 {
@@ -189,6 +190,7 @@ void fw_core_remove_descriptor(struct fw_descriptor *desc)
 
         mutex_unlock(&card_mutex);
 }
+EXPORT_SYMBOL(fw_core_remove_descriptor);
 
 static void allocate_broadcast_channel(struct fw_card *card, int generation)
 {
@@ -459,11 +461,11 @@ EXPORT_SYMBOL(fw_card_add);
 
 
 /*
- * The next few functions implements a dummy driver that use once a
- * card driver shuts down an fw_card.  This allows the driver to
- * cleanly unload, as all IO to the card will be handled by the dummy
- * driver instead of calling into the (possibly) unloaded module.  The
- * dummy driver just fails all IO.
+ * The next few functions implement a dummy driver that is used once a card
+ * driver shuts down an fw_card.  This allows the driver to cleanly unload,
+ * as all IO to the card will be handled (and failed) by the dummy driver
+ * instead of calling into the module.  Only functions for iso context
+ * shutdown still need to be provided by the card driver.
  */
 
 static int dummy_enable(struct fw_card *card, u32 *config_rom, size_t length)
@@ -510,7 +512,7 @@ static int dummy_enable_phys_dma(struct fw_card *card,
         return -ENODEV;
 }
 
-static struct fw_card_driver dummy_driver = {
+static const struct fw_card_driver dummy_driver_template = {
         .enable          = dummy_enable,
         .update_phy_reg  = dummy_update_phy_reg,
         .set_config_rom  = dummy_set_config_rom,
@@ -529,6 +531,8 @@ void fw_card_release(struct kref *kref)
 
 void fw_core_remove_card(struct fw_card *card)
 {
+        struct fw_card_driver dummy_driver = dummy_driver_template;
+
         card->driver->update_phy_reg(card, 4,
                                      PHY_LINK_ACTIVE | PHY_CONTENDER, 0);
         fw_core_initiate_bus_reset(card, 1);
@@ -537,7 +541,9 @@ void fw_core_remove_card(struct fw_card *card)
         list_del_init(&card->link);
         mutex_unlock(&card_mutex);
 
-        /* Set up the dummy driver. */
+        /* Switch off most of the card driver interface. */
+        dummy_driver.free_iso_context   = card->driver->free_iso_context;
+        dummy_driver.stop_iso           = card->driver->stop_iso;
         card->driver = &dummy_driver;
 
         fw_destroy_nodes(card);
diff --git a/drivers/firewire/core-iso.c b/drivers/firewire/core-iso.c
index 28076c892d7e..166f19c6d38d 100644
--- a/drivers/firewire/core-iso.c
+++ b/drivers/firewire/core-iso.c
@@ -71,7 +71,7 @@ int fw_iso_buffer_init(struct fw_iso_buffer *buffer, struct fw_card *card,
         for (j = 0; j < i; j++) {
                 address = page_private(buffer->pages[j]);
                 dma_unmap_page(card->device, address,
-                               PAGE_SIZE, DMA_TO_DEVICE);
+                               PAGE_SIZE, direction);
                 __free_page(buffer->pages[j]);
         }
         kfree(buffer->pages);
@@ -80,6 +80,7 @@ int fw_iso_buffer_init(struct fw_iso_buffer *buffer, struct fw_card *card,
 
         return -ENOMEM;
 }
+EXPORT_SYMBOL(fw_iso_buffer_init);
 
 int fw_iso_buffer_map(struct fw_iso_buffer *buffer, struct vm_area_struct *vma)
 {
@@ -107,13 +108,14 @@ void fw_iso_buffer_destroy(struct fw_iso_buffer *buffer,
         for (i = 0; i < buffer->page_count; i++) {
                 address = page_private(buffer->pages[i]);
                 dma_unmap_page(card->device, address,
-                               PAGE_SIZE, DMA_TO_DEVICE);
+                               PAGE_SIZE, buffer->direction);
                 __free_page(buffer->pages[i]);
         }
 
         kfree(buffer->pages);
         buffer->pages = NULL;
 }
+EXPORT_SYMBOL(fw_iso_buffer_destroy);
 
 struct fw_iso_context *fw_iso_context_create(struct fw_card *card,
                 int type, int channel, int speed, size_t header_size,
@@ -136,6 +138,7 @@ struct fw_iso_context *fw_iso_context_create(struct fw_card *card,
 
         return ctx;
 }
+EXPORT_SYMBOL(fw_iso_context_create);
 
 void fw_iso_context_destroy(struct fw_iso_context *ctx)
 {
@@ -143,12 +146,14 @@ void fw_iso_context_destroy(struct fw_iso_context *ctx)
 
         card->driver->free_iso_context(ctx);
 }
+EXPORT_SYMBOL(fw_iso_context_destroy);
 
 int fw_iso_context_start(struct fw_iso_context *ctx,
                          int cycle, int sync, int tags)
 {
         return ctx->card->driver->start_iso(ctx, cycle, sync, tags);
 }
+EXPORT_SYMBOL(fw_iso_context_start);
 
 int fw_iso_context_queue(struct fw_iso_context *ctx,
                          struct fw_iso_packet *packet,
@@ -159,11 +164,13 @@ int fw_iso_context_queue(struct fw_iso_context *ctx,
 
         return card->driver->queue_iso(ctx, packet, buffer, payload);
 }
+EXPORT_SYMBOL(fw_iso_context_queue);
 
 int fw_iso_context_stop(struct fw_iso_context *ctx)
 {
         return ctx->card->driver->stop_iso(ctx);
 }
+EXPORT_SYMBOL(fw_iso_context_stop);
 
 /*
  * Isochronous bus resource management (channels, bandwidth), client side
diff --git a/drivers/firewire/core.h b/drivers/firewire/core.h
index 0a25a7b38a80..c3cfc647e5e3 100644
--- a/drivers/firewire/core.h
+++ b/drivers/firewire/core.h
@@ -1,7 +1,6 @@
 #ifndef _FIREWIRE_CORE_H
 #define _FIREWIRE_CORE_H
 
-#include <linux/dma-mapping.h>
 #include <linux/fs.h>
 #include <linux/list.h>
 #include <linux/idr.h>
@@ -97,17 +96,6 @@ int fw_core_initiate_bus_reset(struct fw_card *card, int short_reset);
 int fw_compute_block_crc(u32 *block);
 void fw_schedule_bm_work(struct fw_card *card, unsigned long delay);
 
-struct fw_descriptor {
-        struct list_head link;
-        size_t length;
-        u32 immediate;
-        u32 key;
-        const u32 *data;
-};
-
-int fw_core_add_descriptor(struct fw_descriptor *desc);
-void fw_core_remove_descriptor(struct fw_descriptor *desc);
-
 
 /* -cdev */
 
@@ -130,77 +118,7 @@ void fw_node_event(struct fw_card *card, struct fw_node *node, int event);
 
 /* -iso */
 
-/*
- * The iso packet format allows for an immediate header/payload part
- * stored in 'header' immediately after the packet info plus an
- * indirect payload part that is pointer to by the 'payload' field.
- * Applications can use one or the other or both to implement simple
- * low-bandwidth streaming (e.g. audio) or more advanced
- * scatter-gather streaming (e.g. assembling video frame automatically).
- */
-struct fw_iso_packet {
-        u16 payload_length;     /* Length of indirect payload. */
-        u32 interrupt:1;        /* Generate interrupt on this packet */
-        u32 skip:1;             /* Set to not send packet at all. */
-        u32 tag:2;
-        u32 sy:4;
-        u32 header_length:8;    /* Length of immediate header. */
-        u32 header[0];
-};
-
-#define FW_ISO_CONTEXT_TRANSMIT 0
-#define FW_ISO_CONTEXT_RECEIVE  1
-
-#define FW_ISO_CONTEXT_MATCH_TAG0        1
-#define FW_ISO_CONTEXT_MATCH_TAG1        2
-#define FW_ISO_CONTEXT_MATCH_TAG2        4
-#define FW_ISO_CONTEXT_MATCH_TAG3        8
-#define FW_ISO_CONTEXT_MATCH_ALL_TAGS   15
-
-/*
- * An iso buffer is just a set of pages mapped for DMA in the
- * specified direction.  Since the pages are to be used for DMA, they
- * are not mapped into the kernel virtual address space.  We store the
- * DMA address in the page private. The helper function
- * fw_iso_buffer_map() will map the pages into a given vma.
- */
-struct fw_iso_buffer {
-        enum dma_data_direction direction;
-        struct page **pages;
-        int page_count;
-};
-
-typedef void (*fw_iso_callback_t)(struct fw_iso_context *context,
-                                  u32 cycle, size_t header_length,
-                                  void *header, void *data);
-
-struct fw_iso_context {
-        struct fw_card *card;
-        int type;
-        int channel;
-        int speed;
-        size_t header_size;
-        fw_iso_callback_t callback;
-        void *callback_data;
-};
-
-int fw_iso_buffer_init(struct fw_iso_buffer *buffer, struct fw_card *card,
-                       int page_count, enum dma_data_direction direction);
 int fw_iso_buffer_map(struct fw_iso_buffer *buffer, struct vm_area_struct *vma);
-void fw_iso_buffer_destroy(struct fw_iso_buffer *buffer, struct fw_card *card);
-
-struct fw_iso_context *fw_iso_context_create(struct fw_card *card,
-                int type, int channel, int speed, size_t header_size,
-                fw_iso_callback_t callback, void *callback_data);
-int fw_iso_context_queue(struct fw_iso_context *ctx,
-                         struct fw_iso_packet *packet,
-                         struct fw_iso_buffer *buffer,
-                         unsigned long payload);
-int fw_iso_context_start(struct fw_iso_context *ctx,
-                         int cycle, int sync, int tags);
-int fw_iso_context_stop(struct fw_iso_context *ctx);
-void fw_iso_context_destroy(struct fw_iso_context *ctx);
-
 void fw_iso_resource_manage(struct fw_card *card, int generation,
                 u64 channels_mask, int *channel, int *bandwidth, bool allocate);
 
@@ -285,9 +203,4 @@ void fw_flush_transactions(struct fw_card *card);
 void fw_send_phy_config(struct fw_card *card,
                         int node_id, int generation, int gap_count);
 
-static inline int fw_stream_packet_destination_id(int tag, int channel, int sy)
-{
-        return tag << 14 | channel << 8 | sy;
-}
-
 #endif /* _FIREWIRE_CORE_H */
diff --git a/drivers/firewire/net.c b/drivers/firewire/net.c
new file mode 100644
index 000000000000..a42209a73aed
--- /dev/null
+++ b/drivers/firewire/net.c
@@ -0,0 +1,1655 @@
+/*
+ * IPv4 over IEEE 1394, per RFC 2734
+ *
+ * Copyright (C) 2009 Jay Fenlason <fenlason@redhat.com>
+ *
+ * based on eth1394 by Ben Collins et al
+ */
+
+#include <linux/bug.h>
+#include <linux/device.h>
+#include <linux/ethtool.h>
+#include <linux/firewire.h>
+#include <linux/firewire-constants.h>
+#include <linux/highmem.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/jiffies.h>
+#include <linux/mod_devicetable.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/mutex.h>
+#include <linux/netdevice.h>
+#include <linux/skbuff.h>
+#include <linux/spinlock.h>
+
+#include <asm/unaligned.h>
+#include <net/arp.h>
+
+#define FWNET_MAX_FRAGMENTS     25      /* arbitrary limit */
+#define FWNET_ISO_PAGE_COUNT    (PAGE_SIZE < 16 * 1024 ? 4 : 2)
+
+#define IEEE1394_BROADCAST_CHANNEL      31
+#define IEEE1394_ALL_NODES              (0xffc0 | 0x003f)
+#define IEEE1394_MAX_PAYLOAD_S100       512
+#define FWNET_NO_FIFO_ADDR              (~0ULL)
+
+#define IANA_SPECIFIER_ID               0x00005eU
+#define RFC2734_SW_VERSION              0x000001U
+
+#define IEEE1394_GASP_HDR_SIZE  8
+
+#define RFC2374_UNFRAG_HDR_SIZE 4
+#define RFC2374_FRAG_HDR_SIZE   8
+#define RFC2374_FRAG_OVERHEAD   4
+
+#define RFC2374_HDR_UNFRAG      0       /* unfragmented         */
+#define RFC2374_HDR_FIRSTFRAG   1       /* first fragment       */
+#define RFC2374_HDR_LASTFRAG    2       /* last fragment        */
+#define RFC2374_HDR_INTFRAG     3       /* interior fragment    */
+
+#define RFC2734_HW_ADDR_LEN     16
+
+struct rfc2734_arp {
+        __be16 hw_type;         /* 0x0018       */
+        __be16 proto_type;      /* 0x0806       */
+        u8 hw_addr_len;         /* 16           */
+        u8 ip_addr_len;         /* 4            */
+        __be16 opcode;          /* ARP Opcode   */
+        /* Above is exactly the same format as struct arphdr */
+
+        __be64 s_uniq_id;       /* Sender's 64bit EUI                   */
+        u8 max_rec;             /* Sender's max packet size             */
+        u8 sspd;                /* Sender's max speed                   */
+        __be16 fifo_hi;         /* hi 16bits of sender's FIFO addr      */
+        __be32 fifo_lo;         /* lo 32bits of sender's FIFO addr      */
+        __be32 sip;             /* Sender's IP Address                  */
+        __be32 tip;             /* IP Address of requested hw addr      */
+} __attribute__((packed));
+
+/* This header format is specific to this driver implementation. */
+#define FWNET_ALEN      8
+#define FWNET_HLEN      10
+struct fwnet_header {
+        u8 h_dest[FWNET_ALEN];  /* destination address */
+        __be16 h_proto;         /* packet type ID field */
+} __attribute__((packed));
+
+/* IPv4 and IPv6 encapsulation header */
+struct rfc2734_header {
+        u32 w0;
+        u32 w1;
+};
+
+#define fwnet_get_hdr_lf(h)             (((h)->w0 & 0xc0000000) >> 30)
+#define fwnet_get_hdr_ether_type(h)     (((h)->w0 & 0x0000ffff))
+#define fwnet_get_hdr_dg_size(h)        (((h)->w0 & 0x0fff0000) >> 16)
+#define fwnet_get_hdr_fg_off(h)         (((h)->w0 & 0x00000fff))
+#define fwnet_get_hdr_dgl(h)            (((h)->w1 & 0xffff0000) >> 16)
+
+#define fwnet_set_hdr_lf(lf)            ((lf)  << 30)
+#define fwnet_set_hdr_ether_type(et)    (et)
+#define fwnet_set_hdr_dg_size(dgs)      ((dgs) << 16)
+#define fwnet_set_hdr_fg_off(fgo)       (fgo)
+
+#define fwnet_set_hdr_dgl(dgl)          ((dgl) << 16)
+
+static inline void fwnet_make_uf_hdr(struct rfc2734_header *hdr,
+                unsigned ether_type)
+{
+        hdr->w0 = fwnet_set_hdr_lf(RFC2374_HDR_UNFRAG)
+                  | fwnet_set_hdr_ether_type(ether_type);
+}
+
+static inline void fwnet_make_ff_hdr(struct rfc2734_header *hdr,
+                unsigned ether_type, unsigned dg_size, unsigned dgl)
+{
+        hdr->w0 = fwnet_set_hdr_lf(RFC2374_HDR_FIRSTFRAG)
+                  | fwnet_set_hdr_dg_size(dg_size)
+                  | fwnet_set_hdr_ether_type(ether_type);
+        hdr->w1 = fwnet_set_hdr_dgl(dgl);
+}
+
+static inline void fwnet_make_sf_hdr(struct rfc2734_header *hdr,
+                unsigned lf, unsigned dg_size, unsigned fg_off, unsigned dgl)
+{
+        hdr->w0 = fwnet_set_hdr_lf(lf)
+                  | fwnet_set_hdr_dg_size(dg_size)
+                  | fwnet_set_hdr_fg_off(fg_off);
+        hdr->w1 = fwnet_set_hdr_dgl(dgl);
+}
+
+/* This list keeps track of what parts of the datagram have been filled in */
+struct fwnet_fragment_info {
+        struct list_head fi_link;
+        u16 offset;
+        u16 len;
+};
+
+struct fwnet_partial_datagram {
+        struct list_head pd_link;
+        struct list_head fi_list;
+        struct sk_buff *skb;
+        /* FIXME Why not use skb->data? */
+        char *pbuf;
+        u16 datagram_label;
+        u16 ether_type;
+        u16 datagram_size;
+};
+
+static DEFINE_MUTEX(fwnet_device_mutex);
+static LIST_HEAD(fwnet_device_list);
+
+struct fwnet_device {
+        struct list_head dev_link;
+        spinlock_t lock;
+        enum {
+                FWNET_BROADCAST_ERROR,
+                FWNET_BROADCAST_RUNNING,
+                FWNET_BROADCAST_STOPPED,
+        } broadcast_state;
+        struct fw_iso_context *broadcast_rcv_context;
+        struct fw_iso_buffer broadcast_rcv_buffer;
+        void **broadcast_rcv_buffer_ptrs;
+        unsigned broadcast_rcv_next_ptr;
+        unsigned num_broadcast_rcv_ptrs;
+        unsigned rcv_buffer_size;
+        /*
+         * This value is the maximum unfragmented datagram size that can be
+         * sent by the hardware.  It already has the GASP overhead and the
+         * unfragmented datagram header overhead calculated into it.
+         */
+        unsigned broadcast_xmt_max_payload;
+        u16 broadcast_xmt_datagramlabel;
+
+        /*
+         * The CSR address that remote nodes must send datagrams to for us to
+         * receive them.
+         */
+        struct fw_address_handler handler;
+        u64 local_fifo;
+
+        /* List of packets to be sent */
+        struct list_head packet_list;
+        /*
+         * List of packets that were broadcasted.  When we get an ISO interrupt
+         * one of them has been sent
+         */
+        struct list_head broadcasted_list;
+        /* List of packets that have been sent but not yet acked */
+        struct list_head sent_list;
+
+        struct list_head peer_list;
+        struct fw_card *card;
+        struct net_device *netdev;
+};
+
+struct fwnet_peer {
+        struct list_head peer_link;
+        struct fwnet_device *dev;
+        u64 guid;
+        u64 fifo;
+
+        /* guarded by dev->lock */
+        struct list_head pd_list; /* received partial datagrams */
+        unsigned pdg_size;        /* pd_list size */
+
+        u16 datagram_label;       /* outgoing datagram label */
+        unsigned max_payload;     /* includes RFC2374_FRAG_HDR_SIZE overhead */
+        int node_id;
+        int generation;
+        unsigned speed;
+};
+
+/* This is our task struct. It's used for the packet complete callback.  */
+struct fwnet_packet_task {
+        /*
+         * ptask can actually be on dev->packet_list, dev->broadcasted_list,
+         * or dev->sent_list depending on its current state.
+         */
+        struct list_head pt_link;
+        struct fw_transaction transaction;
+        struct rfc2734_header hdr;
+        struct sk_buff *skb;
+        struct fwnet_device *dev;
+
+        int outstanding_pkts;
+        unsigned max_payload;
+        u64 fifo_addr;
+        u16 dest_node;
+        u8 generation;
+        u8 speed;
+};
+
+/*
+ * saddr == NULL means use device source address.
+ * daddr == NULL means leave destination address (eg unresolved arp).
+ */
+static int fwnet_header_create(struct sk_buff *skb, struct net_device *net,
+                        unsigned short type, const void *daddr,
+                        const void *saddr, unsigned len)
+{
+        struct fwnet_header *h;
+
+        h = (struct fwnet_header *)skb_push(skb, sizeof(*h));
+        put_unaligned_be16(type, &h->h_proto);
+
+        if (net->flags & (IFF_LOOPBACK | IFF_NOARP)) {
+                memset(h->h_dest, 0, net->addr_len);
+
+                return net->hard_header_len;
+        }
+
+        if (daddr) {
+                memcpy(h->h_dest, daddr, net->addr_len);
+
+                return net->hard_header_len;
+        }
+
+        return -net->hard_header_len;
+}
+
+static int fwnet_header_rebuild(struct sk_buff *skb)
+{
+        struct fwnet_header *h = (struct fwnet_header *)skb->data;
+
+        if (get_unaligned_be16(&h->h_proto) == ETH_P_IP)
+                return arp_find((unsigned char *)&h->h_dest, skb);
+
+        fw_notify("%s: unable to resolve type %04x addresses\n",
+                  skb->dev->name, be16_to_cpu(h->h_proto));
+        return 0;
+}
+
+static int fwnet_header_cache(const struct neighbour *neigh,
+                              struct hh_cache *hh)
+{
+        struct net_device *net;
+        struct fwnet_header *h;
+
+        if (hh->hh_type == cpu_to_be16(ETH_P_802_3))
+                return -1;
+        net = neigh->dev;
+        h = (struct fwnet_header *)((u8 *)hh->hh_data + 16 - sizeof(*h));
+        h->h_proto = hh->hh_type;
+        memcpy(h->h_dest, neigh->ha, net->addr_len);
+        hh->hh_len = FWNET_HLEN;
+
+        return 0;
+}
+
+/* Called by Address Resolution module to notify changes in address. */
+static void fwnet_header_cache_update(struct hh_cache *hh,
+                const struct net_device *net, const unsigned char *haddr)
+{
+        memcpy((u8 *)hh->hh_data + 16 - FWNET_HLEN, haddr, net->addr_len);
+}
+
+static int fwnet_header_parse(const struct sk_buff *skb, unsigned char *haddr)
+{
+        memcpy(haddr, skb->dev->dev_addr, FWNET_ALEN);
+
+        return FWNET_ALEN;
+}
+
+static const struct header_ops fwnet_header_ops = {
+        .create         = fwnet_header_create,
+        .rebuild        = fwnet_header_rebuild,
+        .cache          = fwnet_header_cache,
+        .cache_update   = fwnet_header_cache_update,
+        .parse          = fwnet_header_parse,
+};
+
+/* FIXME: is this correct for all cases? */
+static bool fwnet_frag_overlap(struct fwnet_partial_datagram *pd,
+                               unsigned offset, unsigned len)
+{
+        struct fwnet_fragment_info *fi;
+        unsigned end = offset + len;
+
+        list_for_each_entry(fi, &pd->fi_list, fi_link)
+                if (offset < fi->offset + fi->len && end > fi->offset)
+                        return true;
+
+        return false;
+}
+
+/* Assumes that new fragment does not overlap any existing fragments */
+static struct fwnet_fragment_info *fwnet_frag_new(
+        struct fwnet_partial_datagram *pd, unsigned offset, unsigned len)
+{
+        struct fwnet_fragment_info *fi, *fi2, *new;
+        struct list_head *list;
+
+        list = &pd->fi_list;
+        list_for_each_entry(fi, &pd->fi_list, fi_link) {
+                if (fi->offset + fi->len == offset) {
+                        /* The new fragment can be tacked on to the end */
+                        /* Did the new fragment plug a hole? */
+                        fi2 = list_entry(fi->fi_link.next,
+                                         struct fwnet_fragment_info, fi_link);
+                        if (fi->offset + fi->len == fi2->offset) {
+                                /* glue fragments together */
+                                fi->len += len + fi2->len;
+                                list_del(&fi2->fi_link);
+                                kfree(fi2);
+                        } else {
+                                fi->len += len;
+                        }
+
+                        return fi;
+                }
+                if (offset + len == fi->offset) {
+                        /* The new fragment can be tacked on to the beginning */
+                        /* Did the new fragment plug a hole? */
+                        fi2 = list_entry(fi->fi_link.prev,
+                                         struct fwnet_fragment_info, fi_link);
+                        if (fi2->offset + fi2->len == fi->offset) {
+                                /* glue fragments together */
+                                fi2->len += fi->len + len;
+                                list_del(&fi->fi_link);
+                                kfree(fi);
+
+                                return fi2;
+                        }
+                        fi->offset = offset;
+                        fi->len += len;
+
+                        return fi;
+                }
+                if (offset > fi->offset + fi->len) {
+                        list = &fi->fi_link;
+                        break;
+                }
+                if (offset + len < fi->offset) {
+                        list = fi->fi_link.prev;
+                        break;
+                }
+        }
+
+        new = kmalloc(sizeof(*new), GFP_ATOMIC);
+        if (!new) {
+                fw_error("out of memory\n");
+                return NULL;
+        }
+
+        new->offset = offset;
+        new->len = len;
+        list_add(&new->fi_link, list);
+
+        return new;
+}
+
+static struct fwnet_partial_datagram *fwnet_pd_new(struct net_device *net,
+                struct fwnet_peer *peer, u16 datagram_label, unsigned dg_size,
+                void *frag_buf, unsigned frag_off, unsigned frag_len)
+{
+        struct fwnet_partial_datagram *new;
+        struct fwnet_fragment_info *fi;
+
+        new = kmalloc(sizeof(*new), GFP_ATOMIC);
+        if (!new)
+                goto fail;
+
+        INIT_LIST_HEAD(&new->fi_list);
+        fi = fwnet_frag_new(new, frag_off, frag_len);
+        if (fi == NULL)
+                goto fail_w_new;
+
+        new->datagram_label = datagram_label;
+        new->datagram_size = dg_size;
+        new->skb = dev_alloc_skb(dg_size + net->hard_header_len + 15);
+        if (new->skb == NULL)
+                goto fail_w_fi;
+
+        skb_reserve(new->skb, (net->hard_header_len + 15) & ~15);
+        new->pbuf = skb_put(new->skb, dg_size);
+        memcpy(new->pbuf + frag_off, frag_buf, frag_len);
+        list_add_tail(&new->pd_link, &peer->pd_list);
+
+        return new;
+
+fail_w_fi:
+        kfree(fi);
+fail_w_new:
+        kfree(new);
+fail:
+        fw_error("out of memory\n");
+
+        return NULL;
+}
+
+static struct fwnet_partial_datagram *fwnet_pd_find(struct fwnet_peer *peer,
+                                                    u16 datagram_label)
+{
+        struct fwnet_partial_datagram *pd;
+
+        list_for_each_entry(pd, &peer->pd_list, pd_link)
+                if (pd->datagram_label == datagram_label)
+                        return pd;
+
+        return NULL;
+}
+
+
+static void fwnet_pd_delete(struct fwnet_partial_datagram *old)
+{
+        struct fwnet_fragment_info *fi, *n;
+
+        list_for_each_entry_safe(fi, n, &old->fi_list, fi_link)
+                kfree(fi);
+
+        list_del(&old->pd_link);
+        dev_kfree_skb_any(old->skb);
+        kfree(old);
+}
+
+static bool fwnet_pd_update(struct fwnet_peer *peer,
+                struct fwnet_partial_datagram *pd, void *frag_buf,
+                unsigned frag_off, unsigned frag_len)
+{
+        if (fwnet_frag_new(pd, frag_off, frag_len) == NULL)
+                return false;
+
+        memcpy(pd->pbuf + frag_off, frag_buf, frag_len);
+
+        /*
+         * Move list entry to beginnig of list so that oldest partial
+         * datagrams percolate to the end of the list
+         */
+        list_move_tail(&pd->pd_link, &peer->pd_list);
+
+        return true;
+}
+
+static bool fwnet_pd_is_complete(struct fwnet_partial_datagram *pd)
+{
+        struct fwnet_fragment_info *fi;
+
+        fi = list_entry(pd->fi_list.next, struct fwnet_fragment_info, fi_link);
+
+        return fi->len == pd->datagram_size;
+}
+
+/* caller must hold dev->lock */
+static struct fwnet_peer *fwnet_peer_find_by_guid(struct fwnet_device *dev,
+                                                  u64 guid)
+{
+        struct fwnet_peer *peer;
+
+        list_for_each_entry(peer, &dev->peer_list, peer_link)
+                if (peer->guid == guid)
+                        return peer;
+
+        return NULL;
+}
+
+/* caller must hold dev->lock */
+static struct fwnet_peer *fwnet_peer_find_by_node_id(struct fwnet_device *dev,
+                                                int node_id, int generation)
+{
+        struct fwnet_peer *peer;
+
+        list_for_each_entry(peer, &dev->peer_list, peer_link)
+                if (peer->node_id    == node_id &&
+                    peer->generation == generation)
+                        return peer;
+
+        return NULL;
+}
+
+/* See IEEE 1394-2008 table 6-4, table 8-8, table 16-18. */
+static unsigned fwnet_max_payload(unsigned max_rec, unsigned speed)
+{
+        max_rec = min(max_rec, speed + 8);
+        max_rec = min(max_rec, 0xbU); /* <= 4096 */
+        if (max_rec < 8) {
+                fw_notify("max_rec %x out of range\n", max_rec);
+                max_rec = 8;
+        }
+
+        return (1 << (max_rec + 1)) - RFC2374_FRAG_HDR_SIZE;
+}
+
+
+static int fwnet_finish_incoming_packet(struct net_device *net,
+                                        struct sk_buff *skb, u16 source_node_id,
+                                        bool is_broadcast, u16 ether_type)
+{
+        struct fwnet_device *dev;
+        static const __be64 broadcast_hw = cpu_to_be64(~0ULL);
+        int status;
+        __be64 guid;
+
+        dev = netdev_priv(net);
+        /* Write metadata, and then pass to the receive level */
+        skb->dev = net;
+        skb->ip_summed = CHECKSUM_UNNECESSARY;  /* don't check it */
+
+        /*
+         * Parse the encapsulation header. This actually does the job of
+         * converting to an ethernet frame header, as well as arp
+         * conversion if needed. ARP conversion is easier in this
+         * direction, since we are using ethernet as our backend.
+         */
+        /*
+         * If this is an ARP packet, convert it. First, we want to make
+         * use of some of the fields, since they tell us a little bit
+         * about the sending machine.
+         */
+        if (ether_type == ETH_P_ARP) {
+                struct rfc2734_arp *arp1394;
+                struct arphdr *arp;
+                unsigned char *arp_ptr;
+                u64 fifo_addr;
+                u64 peer_guid;
+                unsigned sspd;
+                u16 max_payload;
+                struct fwnet_peer *peer;
+                unsigned long flags;
+
+                arp1394   = (struct rfc2734_arp *)skb->data;
+                arp       = (struct arphdr *)skb->data;
+                arp_ptr   = (unsigned char *)(arp + 1);
+                peer_guid = get_unaligned_be64(&arp1394->s_uniq_id);
+                fifo_addr = (u64)get_unaligned_be16(&arp1394->fifo_hi) << 32
+                                | get_unaligned_be32(&arp1394->fifo_lo);
+
+                sspd = arp1394->sspd;
+                /* Sanity check.  OS X 10.3 PPC reportedly sends 131. */
+                if (sspd > SCODE_3200) {
+                        fw_notify("sspd %x out of range\n", sspd);
+                        sspd = SCODE_3200;
+                }
+                max_payload = fwnet_max_payload(arp1394->max_rec, sspd);
+
+                spin_lock_irqsave(&dev->lock, flags);
+                peer = fwnet_peer_find_by_guid(dev, peer_guid);
+                if (peer) {
+                        peer->fifo = fifo_addr;
+
+                        if (peer->speed > sspd)
+                                peer->speed = sspd;
+                        if (peer->max_payload > max_payload)
+                                peer->max_payload = max_payload;
+                }
+                spin_unlock_irqrestore(&dev->lock, flags);
+
+                if (!peer) {
+                        fw_notify("No peer for ARP packet from %016llx\n",
+                                  (unsigned long long)peer_guid);
+                        goto failed_proto;
+                }
+
+                /*
+                 * Now that we're done with the 1394 specific stuff, we'll
+                 * need to alter some of the data.  Believe it or not, all
+                 * that needs to be done is sender_IP_address needs to be
+                 * moved, the destination hardware address get stuffed
+                 * in and the hardware address length set to 8.
+                 *
+                 * IMPORTANT: The code below overwrites 1394 specific data
+                 * needed above so keep the munging of the data for the
+                 * higher level IP stack last.
+                 */
+
+                arp->ar_hln = 8;
+                /* skip over sender unique id */
+                arp_ptr += arp->ar_hln;
+                /* move sender IP addr */
+                put_unaligned(arp1394->sip, (u32 *)arp_ptr);
+                /* skip over sender IP addr */
+                arp_ptr += arp->ar_pln;
+
+                if (arp->ar_op == htons(ARPOP_REQUEST))
+                        memset(arp_ptr, 0, sizeof(u64));
+                else
+                        memcpy(arp_ptr, net->dev_addr, sizeof(u64));
+        }
+
+        /* Now add the ethernet header. */
+        guid = cpu_to_be64(dev->card->guid);
+        if (dev_hard_header(skb, net, ether_type,
+                           is_broadcast ? &broadcast_hw : &guid,
+                           NULL, skb->len) >= 0) {
+                struct fwnet_header *eth;
+                u16 *rawp;
+                __be16 protocol;
+
+                skb_reset_mac_header(skb);
+                skb_pull(skb, sizeof(*eth));
+                eth = (struct fwnet_header *)skb_mac_header(skb);
+                if (*eth->h_dest & 1) {
+                        if (memcmp(eth->h_dest, net->broadcast,
+                                   net->addr_len) == 0)
+                                skb->pkt_type = PACKET_BROADCAST;
+#if 0
+                        else
+                                skb->pkt_type = PACKET_MULTICAST;
+#endif
+                } else {
+                        if (memcmp(eth->h_dest, net->dev_addr, net->addr_len))
+                                skb->pkt_type = PACKET_OTHERHOST;
+                }
+                if (ntohs(eth->h_proto) >= 1536) {
+                        protocol = eth->h_proto;
+                } else {
+                        rawp = (u16 *)skb->data;
+                        if (*rawp == 0xffff)
+                                protocol = htons(ETH_P_802_3);
+                        else
+                                protocol = htons(ETH_P_802_2);
+                }
+                skb->protocol = protocol;
+        }
+        status = netif_rx(skb);
+        if (status == NET_RX_DROP) {
+                net->stats.rx_errors++;
+                net->stats.rx_dropped++;
+        } else {
+                net->stats.rx_packets++;
+                net->stats.rx_bytes += skb->len;
+        }
+        if (netif_queue_stopped(net))
+                netif_wake_queue(net);
+
+        return 0;
+
+ failed_proto:
+        net->stats.rx_errors++;
+        net->stats.rx_dropped++;
+
+        dev_kfree_skb_any(skb);
+        if (netif_queue_stopped(net))
+                netif_wake_queue(net);
+
+        net->last_rx = jiffies;
+
+        return 0;
+}
+
+static int fwnet_incoming_packet(struct fwnet_device *dev, __be32 *buf, int len,
+                                 int source_node_id, int generation,
+                                 bool is_broadcast)
+{
+        struct sk_buff *skb;
+        struct net_device *net = dev->netdev;
+        struct rfc2734_header hdr;
+        unsigned lf;
+        unsigned long flags;
+        struct fwnet_peer *peer;
+        struct fwnet_partial_datagram *pd;
+        int fg_off;
+        int dg_size;
+        u16 datagram_label;
+        int retval;
+        u16 ether_type;
+
+        hdr.w0 = be32_to_cpu(buf[0]);
+        lf = fwnet_get_hdr_lf(&hdr);
+        if (lf == RFC2374_HDR_UNFRAG) {
+                /*
+                 * An unfragmented datagram has been received by the ieee1394
+                 * bus. Build an skbuff around it so we can pass it to the
+                 * high level network layer.
+                 */
+                ether_type = fwnet_get_hdr_ether_type(&hdr);
+                buf++;
+                len -= RFC2374_UNFRAG_HDR_SIZE;
+
+                skb = dev_alloc_skb(len + net->hard_header_len + 15);
+                if (unlikely(!skb)) {
+                        fw_error("out of memory\n");
+                        net->stats.rx_dropped++;
+
+                        return -1;
+                }
+                skb_reserve(skb, (net->hard_header_len + 15) & ~15);
+                memcpy(skb_put(skb, len), buf, len);
+
+                return fwnet_finish_incoming_packet(net, skb, source_node_id,
+                                                    is_broadcast, ether_type);
+        }
+        /* A datagram fragment has been received, now the fun begins. */
+        hdr.w1 = ntohl(buf[1]);
+        buf += 2;
+        len -= RFC2374_FRAG_HDR_SIZE;
+        if (lf == RFC2374_HDR_FIRSTFRAG) {
+                ether_type = fwnet_get_hdr_ether_type(&hdr);
+                fg_off = 0;
+        } else {
+                ether_type = 0;
+                fg_off = fwnet_get_hdr_fg_off(&hdr);
+        }
+        datagram_label = fwnet_get_hdr_dgl(&hdr);
+        dg_size = fwnet_get_hdr_dg_size(&hdr); /* ??? + 1 */
+
+        spin_lock_irqsave(&dev->lock, flags);
+
+        peer = fwnet_peer_find_by_node_id(dev, source_node_id, generation);
+        if (!peer)
+                goto bad_proto;
+
+        pd = fwnet_pd_find(peer, datagram_label);
+        if (pd == NULL) {
+                while (peer->pdg_size >= FWNET_MAX_FRAGMENTS) {
+                        /* remove the oldest */
+                        fwnet_pd_delete(list_first_entry(&peer->pd_list,
+                                struct fwnet_partial_datagram, pd_link));
+                        peer->pdg_size--;
+                }
+                pd = fwnet_pd_new(net, peer, datagram_label,
+                                  dg_size, buf, fg_off, len);
+                if (pd == NULL) {
+                        retval = -ENOMEM;
+                        goto bad_proto;
+                }
+                peer->pdg_size++;
+        } else {
+                if (fwnet_frag_overlap(pd, fg_off, len) ||
+                    pd->datagram_size != dg_size) {
+                        /*
+                         * Differing datagram sizes or overlapping fragments,
+                         * discard old datagram and start a new one.
+                         */
+                        fwnet_pd_delete(pd);
+                        pd = fwnet_pd_new(net, peer, datagram_label,
+                                          dg_size, buf, fg_off, len);
+                        if (pd == NULL) {
+                                retval = -ENOMEM;
+                                peer->pdg_size--;
+                                goto bad_proto;
+                        }
+                } else {
+                        if (!fwnet_pd_update(peer, pd, buf, fg_off, len)) {
+                                /*
+                                 * Couldn't save off fragment anyway
+                                 * so might as well obliterate the
+                                 * datagram now.
+                                 */
+                                fwnet_pd_delete(pd);
+                                peer->pdg_size--;
+                                goto bad_proto;
+                        }
+                }
+        } /* new datagram or add to existing one */
+
+        if (lf == RFC2374_HDR_FIRSTFRAG)
+                pd->ether_type = ether_type;
+
+        if (fwnet_pd_is_complete(pd)) {
+                ether_type = pd->ether_type;
+                peer->pdg_size--;
+                skb = skb_get(pd->skb);
+                fwnet_pd_delete(pd);
+
+                spin_unlock_irqrestore(&dev->lock, flags);
+
+                return fwnet_finish_incoming_packet(net, skb, source_node_id,
+                                                    false, ether_type);
+        }
+        /*
+         * Datagram is not complete, we're done for the
+         * moment.
+         */
+        spin_unlock_irqrestore(&dev->lock, flags);
+
+        return 0;
+
+ bad_proto:
+        spin_unlock_irqrestore(&dev->lock, flags);
+
+        if (netif_queue_stopped(net))
+                netif_wake_queue(net);
+
+        return 0;
+}
+
+static void fwnet_receive_packet(struct fw_card *card, struct fw_request *r,
+                int tcode, int destination, int source, int generation,
+                int speed, unsigned long long offset, void *payload,
+                size_t length, void *callback_data)
+{
+        struct fwnet_device *dev = callback_data;
+        int rcode;
+
+        if (destination == IEEE1394_ALL_NODES) {
+                kfree(r);
+
+                return;
+        }
+
+        if (offset != dev->handler.offset)
+                rcode = RCODE_ADDRESS_ERROR;
+        else if (tcode != TCODE_WRITE_BLOCK_REQUEST)
+                rcode = RCODE_TYPE_ERROR;
+        else if (fwnet_incoming_packet(dev, payload, length,
+                                       source, generation, false) != 0) {
+                fw_error("Incoming packet failure\n");
+                rcode = RCODE_CONFLICT_ERROR;
+        } else
+                rcode = RCODE_COMPLETE;
+
+        fw_send_response(card, r, rcode);
+}
+
+static void fwnet_receive_broadcast(struct fw_iso_context *context,
+                u32 cycle, size_t header_length, void *header, void *data)
+{
+        struct fwnet_device *dev;
+        struct fw_iso_packet packet;
+        struct fw_card *card;
+        __be16 *hdr_ptr;
+        __be32 *buf_ptr;
+        int retval;
+        u32 length;
+        u16 source_node_id;
+        u32 specifier_id;
+        u32 ver;
+        unsigned long offset;
+        unsigned long flags;
+
+        dev = data;
+        card = dev->card;
+        hdr_ptr = header;
+        length = be16_to_cpup(hdr_ptr);
+
+        spin_lock_irqsave(&dev->lock, flags);
+
+        offset = dev->rcv_buffer_size * dev->broadcast_rcv_next_ptr;
+        buf_ptr = dev->broadcast_rcv_buffer_ptrs[dev->broadcast_rcv_next_ptr++];
+        if (dev->broadcast_rcv_next_ptr == dev->num_broadcast_rcv_ptrs)
+                dev->broadcast_rcv_next_ptr = 0;
+
+        spin_unlock_irqrestore(&dev->lock, flags);
+
+        specifier_id =    (be32_to_cpu(buf_ptr[0]) & 0xffff) << 8
+                        | (be32_to_cpu(buf_ptr[1]) & 0xff000000) >> 24;
+        ver = be32_to_cpu(buf_ptr[1]) & 0xffffff;
+        source_node_id = be32_to_cpu(buf_ptr[0]) >> 16;
+
+        if (specifier_id == IANA_SPECIFIER_ID && ver == RFC2734_SW_VERSION) {
+                buf_ptr += 2;
+                length -= IEEE1394_GASP_HDR_SIZE;
+                fwnet_incoming_packet(dev, buf_ptr, length,
+                                      source_node_id, -1, true);
+        }
+
+        packet.payload_length = dev->rcv_buffer_size;
+        packet.interrupt = 1;
+        packet.skip = 0;
+        packet.tag = 3;
+        packet.sy = 0;
+        packet.header_length = IEEE1394_GASP_HDR_SIZE;
+
+        spin_lock_irqsave(&dev->lock, flags);
+
+        retval = fw_iso_context_queue(dev->broadcast_rcv_context, &packet,
+                                      &dev->broadcast_rcv_buffer, offset);
+
+        spin_unlock_irqrestore(&dev->lock, flags);
+
+        if (retval < 0)
+                fw_error("requeue failed\n");
+}
+
+static struct kmem_cache *fwnet_packet_task_cache;
+
+static int fwnet_send_packet(struct fwnet_packet_task *ptask);
+
+static void fwnet_transmit_packet_done(struct fwnet_packet_task *ptask)
+{
+        struct fwnet_device *dev;
+        unsigned long flags;
+
+        dev = ptask->dev;
+
+        spin_lock_irqsave(&dev->lock, flags);
+        list_del(&ptask->pt_link);
+        spin_unlock_irqrestore(&dev->lock, flags);
+
+        ptask->outstanding_pkts--; /* FIXME access inside lock */
+
+        if (ptask->outstanding_pkts > 0) {
+                u16 dg_size;
+                u16 fg_off;
+                u16 datagram_label;
+                u16 lf;
+                struct sk_buff *skb;
+
+                /* Update the ptask to point to the next fragment and send it */
+                lf = fwnet_get_hdr_lf(&ptask->hdr);
+                switch (lf) {
+                case RFC2374_HDR_LASTFRAG:
+                case RFC2374_HDR_UNFRAG:
+                default:
+                        fw_error("Outstanding packet %x lf %x, header %x,%x\n",
+                                 ptask->outstanding_pkts, lf, ptask->hdr.w0,
+                                 ptask->hdr.w1);
+                        BUG();
+
+                case RFC2374_HDR_FIRSTFRAG:
+                        /* Set frag type here for future interior fragments */
+                        dg_size = fwnet_get_hdr_dg_size(&ptask->hdr);
+                        fg_off = ptask->max_payload - RFC2374_FRAG_HDR_SIZE;
+                        datagram_label = fwnet_get_hdr_dgl(&ptask->hdr);
+                        break;
+
+                case RFC2374_HDR_INTFRAG:
+                        dg_size = fwnet_get_hdr_dg_size(&ptask->hdr);
+                        fg_off = fwnet_get_hdr_fg_off(&ptask->hdr)
+                                  + ptask->max_payload - RFC2374_FRAG_HDR_SIZE;
+                        datagram_label = fwnet_get_hdr_dgl(&ptask->hdr);
+                        break;
+                }
+                skb = ptask->skb;
+                skb_pull(skb, ptask->max_payload);
+                if (ptask->outstanding_pkts > 1) {
+                        fwnet_make_sf_hdr(&ptask->hdr, RFC2374_HDR_INTFRAG,
+                                          dg_size, fg_off, datagram_label);
+                } else {
+                        fwnet_make_sf_hdr(&ptask->hdr, RFC2374_HDR_LASTFRAG,
+                                          dg_size, fg_off, datagram_label);
+                        ptask->max_payload = skb->len + RFC2374_FRAG_HDR_SIZE;
+                }
+                fwnet_send_packet(ptask);
+        } else {
+                dev_kfree_skb_any(ptask->skb);
+                kmem_cache_free(fwnet_packet_task_cache, ptask);
+        }
+}
+
+static void fwnet_write_complete(struct fw_card *card, int rcode,
+                                 void *payload, size_t length, void *data)
+{
+        struct fwnet_packet_task *ptask;
+
+        ptask = data;
+
+        if (rcode == RCODE_COMPLETE)
+                fwnet_transmit_packet_done(ptask);
+        else
+                fw_error("fwnet_write_complete: failed: %x\n", rcode);
+                /* ??? error recovery */
+}
+
+static int fwnet_send_packet(struct fwnet_packet_task *ptask)
+{
+        struct fwnet_device *dev;
+        unsigned tx_len;
+        struct rfc2734_header *bufhdr;
+        unsigned long flags;
+
+        dev = ptask->dev;
+        tx_len = ptask->max_payload;
+        switch (fwnet_get_hdr_lf(&ptask->hdr)) {
+        case RFC2374_HDR_UNFRAG:
+                bufhdr = (struct rfc2734_header *)
+                                skb_push(ptask->skb, RFC2374_UNFRAG_HDR_SIZE);
+                put_unaligned_be32(ptask->hdr.w0, &bufhdr->w0);
+                break;
+
+        case RFC2374_HDR_FIRSTFRAG:
+        case RFC2374_HDR_INTFRAG:
+        case RFC2374_HDR_LASTFRAG:
+                bufhdr = (struct rfc2734_header *)
+                                skb_push(ptask->skb, RFC2374_FRAG_HDR_SIZE);
+                put_unaligned_be32(ptask->hdr.w0, &bufhdr->w0);
+                put_unaligned_be32(ptask->hdr.w1, &bufhdr->w1);
+                break;
+
+        default:
+                BUG();
+        }
+        if (ptask->dest_node == IEEE1394_ALL_NODES) {
+                u8 *p;
+                int generation;
+                int node_id;
+
+                /* ptask->generation may not have been set yet */
+                generation = dev->card->generation;
+                smp_rmb();
+                node_id = dev->card->node_id;
+
+                p = skb_push(ptask->skb, 8);
+                put_unaligned_be32(node_id << 16 | IANA_SPECIFIER_ID >> 8, p);
+                put_unaligned_be32((IANA_SPECIFIER_ID & 0xff) << 24
+                                                | RFC2734_SW_VERSION, &p[4]);
+
+                /* We should not transmit if broadcast_channel.valid == 0. */
+                fw_send_request(dev->card, &ptask->transaction,
+                                TCODE_STREAM_DATA,
+                                fw_stream_packet_destination_id(3,
+                                                IEEE1394_BROADCAST_CHANNEL, 0),
+                                generation, SCODE_100, 0ULL, ptask->skb->data,
+                                tx_len + 8, fwnet_write_complete, ptask);
+
+                /* FIXME race? */
+                spin_lock_irqsave(&dev->lock, flags);
+                list_add_tail(&ptask->pt_link, &dev->broadcasted_list);
+                spin_unlock_irqrestore(&dev->lock, flags);
+
+                return 0;
+        }
+
+        fw_send_request(dev->card, &ptask->transaction,
+                        TCODE_WRITE_BLOCK_REQUEST, ptask->dest_node,
+                        ptask->generation, ptask->speed, ptask->fifo_addr,
+                        ptask->skb->data, tx_len, fwnet_write_complete, ptask);
+
+        /* FIXME race? */
+        spin_lock_irqsave(&dev->lock, flags);
+        list_add_tail(&ptask->pt_link, &dev->sent_list);
+        spin_unlock_irqrestore(&dev->lock, flags);
+
+        dev->netdev->trans_start = jiffies;
+
+        return 0;
+}
+
+static int fwnet_broadcast_start(struct fwnet_device *dev)
+{
+        struct fw_iso_context *context;
+        int retval;
+        unsigned num_packets;
+        unsigned max_receive;
+        struct fw_iso_packet packet;
+        unsigned long offset;
+        unsigned u;
+
+        if (dev->local_fifo == FWNET_NO_FIFO_ADDR) {
+                /* outside OHCI posted write area? */
+                static const struct fw_address_region region = {
+                        .start = 0xffff00000000ULL,
+                        .end   = CSR_REGISTER_BASE,
+                };
+
+                dev->handler.length = 4096;
+                dev->handler.address_callback = fwnet_receive_packet;
+                dev->handler.callback_data = dev;
+
+                retval = fw_core_add_address_handler(&dev->handler, &region);
+                if (retval < 0)
+                        goto failed_initial;
+
+                dev->local_fifo = dev->handler.offset;
+        }
+
+        max_receive = 1U << (dev->card->max_receive + 1);
+        num_packets = (FWNET_ISO_PAGE_COUNT * PAGE_SIZE) / max_receive;
+
+        if (!dev->broadcast_rcv_context) {
+                void **ptrptr;
+
+                context = fw_iso_context_create(dev->card,
+                    FW_ISO_CONTEXT_RECEIVE, IEEE1394_BROADCAST_CHANNEL,
+                    dev->card->link_speed, 8, fwnet_receive_broadcast, dev);
+                if (IS_ERR(context)) {
+                        retval = PTR_ERR(context);
+                        goto failed_context_create;
+                }
+
+                retval = fw_iso_buffer_init(&dev->broadcast_rcv_buffer,
+                    dev->card, FWNET_ISO_PAGE_COUNT, DMA_FROM_DEVICE);
+                if (retval < 0)
+                        goto failed_buffer_init;
+
+                ptrptr = kmalloc(sizeof(void *) * num_packets, GFP_KERNEL);
+                if (!ptrptr) {
+                        retval = -ENOMEM;
+                        goto failed_ptrs_alloc;
+                }
+
+                dev->broadcast_rcv_buffer_ptrs = ptrptr;
+                for (u = 0; u < FWNET_ISO_PAGE_COUNT; u++) {
+                        void *ptr;
+                        unsigned v;
+
+                        ptr = kmap(dev->broadcast_rcv_buffer.pages[u]);
+                        for (v = 0; v < num_packets / FWNET_ISO_PAGE_COUNT; v++)
+                                *ptrptr++ = (void *)
+                                                ((char *)ptr + v * max_receive);
+                }
+                dev->broadcast_rcv_context = context;
+        } else {
+                context = dev->broadcast_rcv_context;
+        }
+
+        packet.payload_length = max_receive;
+        packet.interrupt = 1;
+        packet.skip = 0;
+        packet.tag = 3;
+        packet.sy = 0;
+        packet.header_length = IEEE1394_GASP_HDR_SIZE;
+        offset = 0;
+
+        for (u = 0; u < num_packets; u++) {
+                retval = fw_iso_context_queue(context, &packet,
+                                &dev->broadcast_rcv_buffer, offset);
+                if (retval < 0)
+                        goto failed_rcv_queue;
+
+                offset += max_receive;
+        }
+        dev->num_broadcast_rcv_ptrs = num_packets;
+        dev->rcv_buffer_size = max_receive;
+        dev->broadcast_rcv_next_ptr = 0U;
+        retval = fw_iso_context_start(context, -1, 0,
+                        FW_ISO_CONTEXT_MATCH_ALL_TAGS); /* ??? sync */
+        if (retval < 0)
+                goto failed_rcv_queue;
+
+        /* FIXME: adjust it according to the min. speed of all known peers? */
+        dev->broadcast_xmt_max_payload = IEEE1394_MAX_PAYLOAD_S100
+                        - IEEE1394_GASP_HDR_SIZE - RFC2374_UNFRAG_HDR_SIZE;
+        dev->broadcast_state = FWNET_BROADCAST_RUNNING;
+
+        return 0;
+
+ failed_rcv_queue:
+        kfree(dev->broadcast_rcv_buffer_ptrs);
+        dev->broadcast_rcv_buffer_ptrs = NULL;
+ failed_ptrs_alloc:
+        fw_iso_buffer_destroy(&dev->broadcast_rcv_buffer, dev->card);
+ failed_buffer_init:
+        fw_iso_context_destroy(context);
+        dev->broadcast_rcv_context = NULL;
+ failed_context_create:
+        fw_core_remove_address_handler(&dev->handler);
+ failed_initial:
+        dev->local_fifo = FWNET_NO_FIFO_ADDR;
+
+        return retval;
+}
+
+/* ifup */
+static int fwnet_open(struct net_device *net)
+{
+        struct fwnet_device *dev = netdev_priv(net);
+        int ret;
+
+        if (dev->broadcast_state == FWNET_BROADCAST_ERROR) {
+                ret = fwnet_broadcast_start(dev);
+                if (ret)
+                        return ret;
+        }
+        netif_start_queue(net);
+
+        return 0;
+}
+
+/* ifdown */
+static int fwnet_stop(struct net_device *net)
+{
+        netif_stop_queue(net);
+
+        /* Deallocate iso context for use by other applications? */
+
+        return 0;
+}
+
+static int fwnet_tx(struct sk_buff *skb, struct net_device *net)
+{
+        struct fwnet_header hdr_buf;
+        struct fwnet_device *dev = netdev_priv(net);
+        __be16 proto;
+        u16 dest_node;
+        unsigned max_payload;
+        u16 dg_size;
+        u16 *datagram_label_ptr;
+        struct fwnet_packet_task *ptask;
+        struct fwnet_peer *peer;
+        unsigned long flags;
+
+        ptask = kmem_cache_alloc(fwnet_packet_task_cache, GFP_ATOMIC);
+        if (ptask == NULL)
+                goto fail;
+
+        skb = skb_share_check(skb, GFP_ATOMIC);
+        if (!skb)
+                goto fail;
+
+        /*
+         * Make a copy of the driver-specific header.
+         * We might need to rebuild the header on tx failure.
+         */
+        memcpy(&hdr_buf, skb->data, sizeof(hdr_buf));
+        skb_pull(skb, sizeof(hdr_buf));
+
+        proto = hdr_buf.h_proto;
+        dg_size = skb->len;
+
+        /* serialize access to peer, including peer->datagram_label */
+        spin_lock_irqsave(&dev->lock, flags);
+
+        /*
+         * Set the transmission type for the packet.  ARP packets and IP
+         * broadcast packets are sent via GASP.
+         */
+        if (memcmp(hdr_buf.h_dest, net->broadcast, FWNET_ALEN) == 0
+            || proto == htons(ETH_P_ARP)
+            || (proto == htons(ETH_P_IP)
+                && IN_MULTICAST(ntohl(ip_hdr(skb)->daddr)))) {
+                max_payload        = dev->broadcast_xmt_max_payload;
+                datagram_label_ptr = &dev->broadcast_xmt_datagramlabel;
+
+                ptask->fifo_addr   = FWNET_NO_FIFO_ADDR;
+                ptask->generation  = 0;
+                ptask->dest_node   = IEEE1394_ALL_NODES;
+                ptask->speed       = SCODE_100;
+        } else {
+                __be64 guid = get_unaligned((__be64 *)hdr_buf.h_dest);
+                u8 generation;
+
+                peer = fwnet_peer_find_by_guid(dev, be64_to_cpu(guid));
+                if (!peer || peer->fifo == FWNET_NO_FIFO_ADDR)
+                        goto fail_unlock;
+
+                generation         = peer->generation;
+                dest_node          = peer->node_id;
+                max_payload        = peer->max_payload;
+                datagram_label_ptr = &peer->datagram_label;
+
+                ptask->fifo_addr   = peer->fifo;
+                ptask->generation  = generation;
+                ptask->dest_node   = dest_node;
+                ptask->speed       = peer->speed;
+        }
+
+        /* If this is an ARP packet, convert it */
+        if (proto == htons(ETH_P_ARP)) {
+                struct arphdr *arp = (struct arphdr *)skb->data;
+                unsigned char *arp_ptr = (unsigned char *)(arp + 1);
+                struct rfc2734_arp *arp1394 = (struct rfc2734_arp *)skb->data;
+                __be32 ipaddr;
+
+                ipaddr = get_unaligned((__be32 *)(arp_ptr + FWNET_ALEN));
+
+                arp1394->hw_addr_len    = RFC2734_HW_ADDR_LEN;
+                arp1394->max_rec        = dev->card->max_receive;
+                arp1394->sspd           = dev->card->link_speed;
+
+                put_unaligned_be16(dev->local_fifo >> 32,
+                                   &arp1394->fifo_hi);
+                put_unaligned_be32(dev->local_fifo & 0xffffffff,
+                                   &arp1394->fifo_lo);
+                put_unaligned(ipaddr, &arp1394->sip);
+        }
+
+        ptask->hdr.w0 = 0;
+        ptask->hdr.w1 = 0;
+        ptask->skb = skb;
+        ptask->dev = dev;
+
+        /* Does it all fit in one packet? */
+        if (dg_size <= max_payload) {
+                fwnet_make_uf_hdr(&ptask->hdr, ntohs(proto));
+                ptask->outstanding_pkts = 1;
+                max_payload = dg_size + RFC2374_UNFRAG_HDR_SIZE;
+        } else {
+                u16 datagram_label;
+
+                max_payload -= RFC2374_FRAG_OVERHEAD;
+                datagram_label = (*datagram_label_ptr)++;
+                fwnet_make_ff_hdr(&ptask->hdr, ntohs(proto), dg_size,
+                                  datagram_label);
+                ptask->outstanding_pkts = DIV_ROUND_UP(dg_size, max_payload);
+                max_payload += RFC2374_FRAG_HDR_SIZE;
+        }
+
+        spin_unlock_irqrestore(&dev->lock, flags);
+
+        ptask->max_payload = max_payload;
+        fwnet_send_packet(ptask);
+
+        return NETDEV_TX_OK;
+
+ fail_unlock:
+        spin_unlock_irqrestore(&dev->lock, flags);
+ fail:
+        if (ptask)
+                kmem_cache_free(fwnet_packet_task_cache, ptask);
+
+        if (skb != NULL)
+                dev_kfree_skb(skb);
+
+        net->stats.tx_dropped++;
+        net->stats.tx_errors++;
+
+        /*
+         * FIXME: According to a patch from 2003-02-26, "returning non-zero
+         * causes serious problems" here, allegedly.  Before that patch,
+         * -ERRNO was returned which is not appropriate under Linux 2.6.
+         * Perhaps more needs to be done?  Stop the queue in serious
+         * conditions and restart it elsewhere?
+         */
+        return NETDEV_TX_OK;
+}
+
+static int fwnet_change_mtu(struct net_device *net, int new_mtu)
+{
+        if (new_mtu < 68)
+                return -EINVAL;
+
+        net->mtu = new_mtu;
+        return 0;
+}
+
+static void fwnet_get_drvinfo(struct net_device *net,
+                              struct ethtool_drvinfo *info)
+{
+        strcpy(info->driver, KBUILD_MODNAME);
+        strcpy(info->bus_info, "ieee1394");
+}
+
+static struct ethtool_ops fwnet_ethtool_ops = {
+        .get_drvinfo = fwnet_get_drvinfo,
+};
+
+static const struct net_device_ops fwnet_netdev_ops = {
+        .ndo_open       = fwnet_open,
+        .ndo_stop       = fwnet_stop,
+        .ndo_start_xmit = fwnet_tx,
+        .ndo_change_mtu = fwnet_change_mtu,
+};
+
+static void fwnet_init_dev(struct net_device *net)
+{
+        net->header_ops         = &fwnet_header_ops;
+        net->netdev_ops         = &fwnet_netdev_ops;
+        net->watchdog_timeo     = 2 * HZ;
+        net->flags              = IFF_BROADCAST | IFF_MULTICAST;
+        net->features           = NETIF_F_HIGHDMA;
+        net->addr_len           = FWNET_ALEN;
+        net->hard_header_len    = FWNET_HLEN;
+        net->type               = ARPHRD_IEEE1394;
+        net->tx_queue_len       = 10;
+        SET_ETHTOOL_OPS(net, &fwnet_ethtool_ops);
+}
+
+/* caller must hold fwnet_device_mutex */
+static struct fwnet_device *fwnet_dev_find(struct fw_card *card)
+{
+        struct fwnet_device *dev;
+
+        list_for_each_entry(dev, &fwnet_device_list, dev_link)
+                if (dev->card == card)
+                        return dev;
+
+        return NULL;
+}
+
+static int fwnet_add_peer(struct fwnet_device *dev,
+                          struct fw_unit *unit, struct fw_device *device)
+{
+        struct fwnet_peer *peer;
+
+        peer = kmalloc(sizeof(*peer), GFP_KERNEL);
+        if (!peer)
+                return -ENOMEM;
+
+        dev_set_drvdata(&unit->device, peer);
+
+        peer->dev = dev;
+        peer->guid = (u64)device->config_rom[3] << 32 | device->config_rom[4];
+        peer->fifo = FWNET_NO_FIFO_ADDR;
+        INIT_LIST_HEAD(&peer->pd_list);
+        peer->pdg_size = 0;
+        peer->datagram_label = 0;
+        peer->speed = device->max_speed;
+        peer->max_payload = fwnet_max_payload(device->max_rec, peer->speed);
+
+        peer->generation = device->generation;
+        smp_rmb();
+        peer->node_id = device->node_id;
+
+        spin_lock_irq(&dev->lock);
+        list_add_tail(&peer->peer_link, &dev->peer_list);
+        spin_unlock_irq(&dev->lock);
+
+        return 0;
+}
+
+static int fwnet_probe(struct device *_dev)
+{
+        struct fw_unit *unit = fw_unit(_dev);
+        struct fw_device *device = fw_parent_device(unit);
+        struct fw_card *card = device->card;
+        struct net_device *net;
+        bool allocated_netdev = false;
+        struct fwnet_device *dev;
+        unsigned max_mtu;
+        int ret;
+
+        mutex_lock(&fwnet_device_mutex);
+
+        dev = fwnet_dev_find(card);
+        if (dev) {
+                net = dev->netdev;
+                goto have_dev;
+        }
+
+        net = alloc_netdev(sizeof(*dev), "firewire%d", fwnet_init_dev);
+        if (net == NULL) {
+                ret = -ENOMEM;
+                goto out;
+        }
+
+        allocated_netdev = true;
+        SET_NETDEV_DEV(net, card->device);
+        dev = netdev_priv(net);
+
+        spin_lock_init(&dev->lock);
+        dev->broadcast_state = FWNET_BROADCAST_ERROR;
+        dev->broadcast_rcv_context = NULL;
+        dev->broadcast_xmt_max_payload = 0;
+        dev->broadcast_xmt_datagramlabel = 0;
+
+        dev->local_fifo = FWNET_NO_FIFO_ADDR;
+
+        INIT_LIST_HEAD(&dev->packet_list);
+        INIT_LIST_HEAD(&dev->broadcasted_list);
+        INIT_LIST_HEAD(&dev->sent_list);
+        INIT_LIST_HEAD(&dev->peer_list);
+
+        dev->card = card;
+        dev->netdev = net;
+
+        /*
+         * Use the RFC 2734 default 1500 octets or the maximum payload
+         * as initial MTU
+         */
+        max_mtu = (1 << (card->max_receive + 1))
+                  - sizeof(struct rfc2734_header) - IEEE1394_GASP_HDR_SIZE;
+        net->mtu = min(1500U, max_mtu);
+
+        /* Set our hardware address while we're at it */
+        put_unaligned_be64(card->guid, net->dev_addr);
+        put_unaligned_be64(~0ULL, net->broadcast);
+        ret = register_netdev(net);
+        if (ret) {
+                fw_error("Cannot register the driver\n");
+                goto out;
+        }
+
+        list_add_tail(&dev->dev_link, &fwnet_device_list);
+        fw_notify("%s: IPv4 over FireWire on device %016llx\n",
+                  net->name, (unsigned long long)card->guid);
+ have_dev:
+        ret = fwnet_add_peer(dev, unit, device);
+        if (ret && allocated_netdev) {
+                unregister_netdev(net);
+                list_del(&dev->dev_link);
+        }
+ out:
+        if (ret && allocated_netdev)
+                free_netdev(net);
+
+        mutex_unlock(&fwnet_device_mutex);
+
+        return ret;
+}
+
+static void fwnet_remove_peer(struct fwnet_peer *peer)
+{
+        struct fwnet_partial_datagram *pd, *pd_next;
+
+        spin_lock_irq(&peer->dev->lock);
+        list_del(&peer->peer_link);
+        spin_unlock_irq(&peer->dev->lock);
+
+        list_for_each_entry_safe(pd, pd_next, &peer->pd_list, pd_link)
+                fwnet_pd_delete(pd);
+
+        kfree(peer);
+}
+
+static int fwnet_remove(struct device *_dev)
+{
+        struct fwnet_peer *peer = dev_get_drvdata(_dev);
+        struct fwnet_device *dev = peer->dev;
+        struct net_device *net;
+        struct fwnet_packet_task *ptask, *pt_next;
+
+        mutex_lock(&fwnet_device_mutex);
+
+        fwnet_remove_peer(peer);
+
+        if (list_empty(&dev->peer_list)) {
+                net = dev->netdev;
+                unregister_netdev(net);
+
+                if (dev->local_fifo != FWNET_NO_FIFO_ADDR)
+                        fw_core_remove_address_handler(&dev->handler);
+                if (dev->broadcast_rcv_context) {
+                        fw_iso_context_stop(dev->broadcast_rcv_context);
+                        fw_iso_buffer_destroy(&dev->broadcast_rcv_buffer,
+                                              dev->card);
+                        fw_iso_context_destroy(dev->broadcast_rcv_context);
+                }
+                list_for_each_entry_safe(ptask, pt_next,
+                                         &dev->packet_list, pt_link) {
+                        dev_kfree_skb_any(ptask->skb);
+                        kmem_cache_free(fwnet_packet_task_cache, ptask);
+                }
+                list_for_each_entry_safe(ptask, pt_next,
+                                         &dev->broadcasted_list, pt_link) {
+                        dev_kfree_skb_any(ptask->skb);
+                        kmem_cache_free(fwnet_packet_task_cache, ptask);
+                }
+                list_for_each_entry_safe(ptask, pt_next,
+                                         &dev->sent_list, pt_link) {
+                        dev_kfree_skb_any(ptask->skb);
+                        kmem_cache_free(fwnet_packet_task_cache, ptask);
+                }
+                list_del(&dev->dev_link);
+
+                free_netdev(net);
+        }
+
+        mutex_unlock(&fwnet_device_mutex);
+
+        return 0;
+}
+
+/*
+ * FIXME abort partially sent fragmented datagrams,
+ * discard partially received fragmented datagrams
+ */
+static void fwnet_update(struct fw_unit *unit)
+{
+        struct fw_device *device = fw_parent_device(unit);
+        struct fwnet_peer *peer = dev_get_drvdata(&unit->device);
+        int generation;
+
+        generation = device->generation;
+
+        spin_lock_irq(&peer->dev->lock);
+        peer->node_id    = device->node_id;
+        peer->generation = generation;
+        spin_unlock_irq(&peer->dev->lock);
+}
+
+static const struct ieee1394_device_id fwnet_id_table[] = {
+        {
+                .match_flags  = IEEE1394_MATCH_SPECIFIER_ID |
+                                IEEE1394_MATCH_VERSION,
+                .specifier_id = IANA_SPECIFIER_ID,
+                .version      = RFC2734_SW_VERSION,
+        },
+        { }
+};
+
+static struct fw_driver fwnet_driver = {
+        .driver = {
+                .owner  = THIS_MODULE,
+                .name   = "net",
+                .bus    = &fw_bus_type,
+                .probe  = fwnet_probe,
+                .remove = fwnet_remove,
+        },
+        .update   = fwnet_update,
+        .id_table = fwnet_id_table,
+};
+
+static const u32 rfc2374_unit_directory_data[] = {
+        0x00040000,     /* directory_length             */
+        0x1200005e,     /* unit_specifier_id: IANA      */
+        0x81000003,     /* textual descriptor offset    */
+        0x13000001,     /* unit_sw_version: RFC 2734    */
+        0x81000005,     /* textual descriptor offset    */
+        0x00030000,     /* descriptor_length            */
+        0x00000000,     /* text                         */
+        0x00000000,     /* minimal ASCII, en            */
+        0x49414e41,     /* I A N A                      */
+        0x00030000,     /* descriptor_length            */
+        0x00000000,     /* text                         */
+        0x00000000,     /* minimal ASCII, en            */
+        0x49507634,     /* I P v 4                      */
+};
+
+static struct fw_descriptor rfc2374_unit_directory = {
+        .length = ARRAY_SIZE(rfc2374_unit_directory_data),
+        .key    = (CSR_DIRECTORY | CSR_UNIT) << 24,
+        .data   = rfc2374_unit_directory_data
+};
+
+static int __init fwnet_init(void)
+{
+        int err;
+
+        err = fw_core_add_descriptor(&rfc2374_unit_directory);
+        if (err)
+                return err;
+
+        fwnet_packet_task_cache = kmem_cache_create("packet_task",
+                        sizeof(struct fwnet_packet_task), 0, 0, NULL);
+        if (!fwnet_packet_task_cache) {
+                err = -ENOMEM;
+                goto out;
+        }
+
+        err = driver_register(&fwnet_driver.driver);
+        if (!err)
+                return 0;
+
+        kmem_cache_destroy(fwnet_packet_task_cache);
+out:
+        fw_core_remove_descriptor(&rfc2374_unit_directory);
+
+        return err;
+}
+module_init(fwnet_init);
+
+static void __exit fwnet_cleanup(void)
+{
+        driver_unregister(&fwnet_driver.driver);
+        kmem_cache_destroy(fwnet_packet_task_cache);
+        fw_core_remove_descriptor(&rfc2374_unit_directory);
+}
+module_exit(fwnet_cleanup);
+
+MODULE_AUTHOR("Jay Fenlason <fenlason@redhat.com>");
+MODULE_DESCRIPTION("IPv4 over IEEE1394 as per RFC 2734");
+MODULE_LICENSE("GPL");
+MODULE_DEVICE_TABLE(ieee1394, fwnet_id_table);
diff --git a/drivers/i2c/busses/i2c-cpm.c b/drivers/i2c/busses/i2c-cpm.c
index b5db8b883615..9c2e10082b79 100644
--- a/drivers/i2c/busses/i2c-cpm.c
+++ b/drivers/i2c/busses/i2c-cpm.c
@@ -140,7 +140,7 @@ static irqreturn_t cpm_i2c_interrupt(int irq, void *dev_id)
 
         dev_dbg(&adap->dev, "Interrupt: %x\n", i);
 
-        wake_up_interruptible(&cpm->i2c_wait);
+        wake_up(&cpm->i2c_wait);
 
         return i ? IRQ_HANDLED : IRQ_NONE;
 }
@@ -364,12 +364,12 @@ static int cpm_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
                 dev_dbg(&adap->dev, "test ready.\n");
                 pmsg = &msgs[tptr];
                 if (pmsg->flags & I2C_M_RD)
-                        ret = wait_event_interruptible_timeout(cpm->i2c_wait,
+                        ret = wait_event_timeout(cpm->i2c_wait,
                                 (in_be16(&tbdf[tptr].cbd_sc) & BD_SC_NAK) ||
                                 !(in_be16(&rbdf[rptr].cbd_sc) & BD_SC_EMPTY),
                                 1 * HZ);
                 else
-                        ret = wait_event_interruptible_timeout(cpm->i2c_wait,
+                        ret = wait_event_timeout(cpm->i2c_wait,
                                 !(in_be16(&tbdf[tptr].cbd_sc) & BD_SC_READY),
                                 1 * HZ);
                 if (ret == 0) {
diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index b606db85525d..ad8d2010c921 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -339,7 +339,7 @@ static int omap_i2c_init(struct omap_i2c_dev *dev)
                  * to get longer filter period for better noise suppression.
                  * The filter is iclk (fclk for HS) period.
                  */
-                if (dev->speed > 400 || cpu_is_omap_2430())
+                if (dev->speed > 400 || cpu_is_omap2430())
                         internal_clk = 19200;
                 else if (dev->speed > 100)
                         internal_clk = 9600;
diff --git a/drivers/ide/cmd64x.c b/drivers/ide/cmd64x.c
index 03c86209446f..680e5975217f 100644
--- a/drivers/ide/cmd64x.c
+++ b/drivers/ide/cmd64x.c
@@ -389,8 +389,7 @@ static const struct ide_port_info cmd64x_chipsets[] __devinitdata = {
                 .init_chipset   = init_chipset_cmd64x,
                 .enablebits     = {{0x51,0x04,0x04}, {0x51,0x08,0x08}},
                 .port_ops       = &cmd648_port_ops,
-                .host_flags     = IDE_HFLAG_SERIALIZE |
-                                  IDE_HFLAG_ABUSE_PREFETCH,
+                .host_flags     = IDE_HFLAG_ABUSE_PREFETCH,
                 .pio_mask       = ATA_PIO5,
                 .mwdma_mask     = ATA_MWDMA2,
                 .udma_mask      = ATA_UDMA2,
diff --git a/drivers/ieee1394/Kconfig b/drivers/ieee1394/Kconfig
index 95f45f9b8e5e..f102fcc7e52a 100644
--- a/drivers/ieee1394/Kconfig
+++ b/drivers/ieee1394/Kconfig
@@ -4,7 +4,7 @@ menu "IEEE 1394 (FireWire) support"
 source "drivers/firewire/Kconfig"
 
 config IEEE1394
-        tristate "Stable FireWire stack"
+        tristate "Legacy alternative FireWire driver stack"
         depends on PCI || BROKEN
         help
           IEEE 1394 describes a high performance serial bus, which is also
@@ -33,11 +33,9 @@ config IEEE1394_OHCI1394
           module will be called ohci1394.
 
           NOTE:
-
-          You should only build either ohci1394 or the new firewire-ohci driver,
-          but not both.  If you nevertheless want to install both, you should
-          configure them only as modules and blacklist the driver(s) which you
-          don't want to have auto-loaded.  Add either
+          If you want to install firewire-ohci and ohci1394 together, you
+          should configure them only as modules and blacklist the driver(s)
+          which you don't want to have auto-loaded.  Add either
 
               blacklist firewire-ohci
           or
@@ -46,12 +44,7 @@ config IEEE1394_OHCI1394
               blacklist dv1394
 
           to /etc/modprobe.conf or /etc/modprobe.d/* and update modprobe.conf
-          depending on your distribution.  The latter two modules should be
-          blacklisted together with ohci1394 because they depend on ohci1394.
-
-          If you have an old modprobe which doesn't implement the blacklist
-          directive, use "install modulename /bin/true" for the modules to be
-          blacklisted.
+          depending on your distribution.
 
 comment "PCILynx controller requires I2C"
         depends on IEEE1394 && I2C=n
@@ -105,7 +98,7 @@ config IEEE1394_ETH1394_ROM_ENTRY
         default n
 
 config IEEE1394_ETH1394
-        tristate "IP over 1394"
+        tristate "IP networking over 1394 (experimental)"
         depends on IEEE1394 && EXPERIMENTAL && INET
         select IEEE1394_ETH1394_ROM_ENTRY
         help
diff --git a/drivers/media/common/ir-keymaps.c b/drivers/media/common/ir-keymaps.c
index 3fe158ac7bbf..4216328552f6 100644
--- a/drivers/media/common/ir-keymaps.c
+++ b/drivers/media/common/ir-keymaps.c
@@ -2750,3 +2750,26 @@ IR_KEYTAB_TYPE ir_codes_dm1105_nec[IR_KEYTAB_SIZE] = {
         [0x1b] = KEY_B,         /*recall*/
 };
 EXPORT_SYMBOL_GPL(ir_codes_dm1105_nec);
+
+/* EVGA inDtube
+   Devin Heitmueller <devin.heitmueller@gmail.com>
+ */
+IR_KEYTAB_TYPE ir_codes_evga_indtube[IR_KEYTAB_SIZE] = {
+        [0x12] = KEY_POWER,
+        [0x02] = KEY_MODE,      /* TV */
+        [0x14] = KEY_MUTE,
+        [0x1a] = KEY_CHANNELUP,
+        [0x16] = KEY_TV2,       /* PIP */
+        [0x1d] = KEY_VOLUMEUP,
+        [0x05] = KEY_CHANNELDOWN,
+        [0x0f] = KEY_PLAYPAUSE,
+        [0x19] = KEY_VOLUMEDOWN,
+        [0x1c] = KEY_REWIND,
+        [0x0d] = KEY_RECORD,
+        [0x18] = KEY_FORWARD,
+        [0x1e] = KEY_PREVIOUS,
+        [0x1b] = KEY_STOP,
+        [0x1f] = KEY_NEXT,
+        [0x13] = KEY_CAMERA,
+};
+EXPORT_SYMBOL_GPL(ir_codes_evga_indtube);
diff --git a/drivers/media/dvb/frontends/stv0900.h b/drivers/media/dvb/frontends/stv0900.h
index 8a1332c2031d..bf4e9b633044 100644
--- a/drivers/media/dvb/frontends/stv0900.h
+++ b/drivers/media/dvb/frontends/stv0900.h
@@ -29,6 +29,11 @@
 #include <linux/dvb/frontend.h>
 #include "dvb_frontend.h"
 
+struct stv0900_reg {
+        u16 addr;
+        u8  val;
+};
+
 struct stv0900_config {
         u8 demod_address;
         u32 xtal;
@@ -38,7 +43,7 @@ struct stv0900_config {
 
         u8 path1_mode;
         u8 path2_mode;
-
+        struct stv0900_reg *ts_config_regs;
         u8 tun1_maddress;/* 0, 1, 2, 3 for 0xc0, 0xc2, 0xc4, 0xc6 */
         u8 tun2_maddress;
         u8 tun1_adc;/* 1 for stv6110, 2 for stb6100 */
diff --git a/drivers/media/dvb/frontends/stv0900_core.c b/drivers/media/dvb/frontends/stv0900_core.c
index 8499bcf7f251..1da045fbb4ef 100644
--- a/drivers/media/dvb/frontends/stv0900_core.c
+++ b/drivers/media/dvb/frontends/stv0900_core.c
@@ -149,31 +149,31 @@ void stv0900_write_reg(struct stv0900_internal *i_params, u16 reg_addr,
                 dprintk(KERN_ERR "%s: i2c error %d\n", __func__, ret);
 }
 
-u8 stv0900_read_reg(struct stv0900_internal *i_params, u16 reg_addr)
+u8 stv0900_read_reg(struct stv0900_internal *i_params, u16 reg)
 {
-        u8 data[2];
         int ret;
-        struct i2c_msg i2cmsg = {
-                .addr  = i_params->i2c_addr,
-                .flags = 0,
-                .len   = 2,
-                .buf   = data,
+        u8 b0[] = { MSB(reg), LSB(reg) };
+        u8 buf = 0;
+        struct i2c_msg msg[] = {
+                {
+                        .addr   = i_params->i2c_addr,
+                        .flags  = 0,
+                        .buf = b0,
+                        .len = 2,
+                }, {
+                        .addr   = i_params->i2c_addr,
+                        .flags  = I2C_M_RD,
+                        .buf = &buf,
+                        .len = 1,
+                },
         };
 
-        data[0] = MSB(reg_addr);
-        data[1] = LSB(reg_addr);
-
-        ret = i2c_transfer(i_params->i2c_adap, &i2cmsg, 1);
-        if (ret != 1)
-                dprintk(KERN_ERR "%s: i2c error %d\n", __func__, ret);
-
-        i2cmsg.flags = I2C_M_RD;
-        i2cmsg.len = 1;
-        ret = i2c_transfer(i_params->i2c_adap, &i2cmsg, 1);
-        if (ret != 1)
-                dprintk(KERN_ERR "%s: i2c error %d\n", __func__, ret);
+        ret = i2c_transfer(i_params->i2c_adap, msg, 2);
+        if (ret != 2)
+                dprintk(KERN_ERR "%s: i2c error %d, reg[0x%02x]\n",
+                                __func__, ret, reg);
 
-        return data[0];
+        return buf;
 }
 
 void extract_mask_pos(u32 label, u8 *mask, u8 *pos)
@@ -712,6 +712,44 @@ static s32 stv0900_carr_get_quality(struct dvb_frontend *fe,
         return c_n;
 }
 
+static int stv0900_read_ucblocks(struct dvb_frontend *fe, u32 * ucblocks)
+{
+        struct stv0900_state *state = fe->demodulator_priv;
+        struct stv0900_internal *i_params = state->internal;
+        enum fe_stv0900_demod_num demod = state->demod;
+        u8 err_val1, err_val0;
+        s32 err_field1, err_field0;
+        u32 header_err_val = 0;
+
+        *ucblocks = 0x0;
+        if (stv0900_get_standard(fe, demod) == STV0900_DVBS2_STANDARD) {
+                /* DVB-S2 delineator errors count */
+
+                /* retreiving number for errnous headers */
+                dmd_reg(err_field0, R0900_P1_BBFCRCKO0,
+                                        R0900_P2_BBFCRCKO0);
+                dmd_reg(err_field1, R0900_P1_BBFCRCKO1,
+                                        R0900_P2_BBFCRCKO1);
+
+                err_val1 = stv0900_read_reg(i_params, err_field1);
+                err_val0 = stv0900_read_reg(i_params, err_field0);
+                header_err_val = (err_val1<<8) | err_val0;
+
+                /* retreiving number for errnous packets */
+                dmd_reg(err_field0, R0900_P1_UPCRCKO0,
+                                        R0900_P2_UPCRCKO0);
+                dmd_reg(err_field1, R0900_P1_UPCRCKO1,
+                                        R0900_P2_UPCRCKO1);
+
+                err_val1 = stv0900_read_reg(i_params, err_field1);
+                err_val0 = stv0900_read_reg(i_params, err_field0);
+                *ucblocks = (err_val1<<8) | err_val0;
+                *ucblocks += header_err_val;
+        }
+
+        return 0;
+}
+
 static int stv0900_read_snr(struct dvb_frontend *fe, u16 *snr)
 {
         *snr = stv0900_carr_get_quality(fe,
@@ -1355,7 +1393,7 @@ static enum fe_stv0900_error stv0900_init_internal(struct dvb_frontend *fe,
         struct stv0900_state *state = fe->demodulator_priv;
         enum fe_stv0900_error error = STV0900_NO_ERROR;
         enum fe_stv0900_error demodError = STV0900_NO_ERROR;
-        int selosci;
+        int selosci, i;
 
         struct stv0900_inode *temp_int = find_inode(state->i2c_adap,
                                                 state->config->demod_address);
@@ -1402,7 +1440,23 @@ static enum fe_stv0900_error stv0900_init_internal(struct dvb_frontend *fe,
                                 stv0900_write_bits(state->internal, F0900_P1_ROLLOFF_CONTROL, p_init->rolloff);
                                 stv0900_write_bits(state->internal, F0900_P2_ROLLOFF_CONTROL, p_init->rolloff);
 
-                                stv0900_set_ts_parallel_serial(state->internal, p_init->path1_ts_clock, p_init->path2_ts_clock);
+                                state->internal->ts_config = p_init->ts_config;
+                                if (state->internal->ts_config == NULL)
+                                        stv0900_set_ts_parallel_serial(state->internal,
+                                                        p_init->path1_ts_clock,
+                                                        p_init->path2_ts_clock);
+                                else {
+                                        for (i = 0; state->internal->ts_config[i].addr != 0xffff; i++)
+                                                stv0900_write_reg(state->internal,
+                                                                state->internal->ts_config[i].addr,
+                                                                state->internal->ts_config[i].val);
+
+                                        stv0900_write_bits(state->internal, F0900_P2_RST_HWARE, 1);
+                                        stv0900_write_bits(state->internal, F0900_P2_RST_HWARE, 0);
+                                        stv0900_write_bits(state->internal, F0900_P1_RST_HWARE, 1);
+                                        stv0900_write_bits(state->internal, F0900_P1_RST_HWARE, 0);
+                                }
+
                                 stv0900_write_bits(state->internal, F0900_P1_TUN_MADDRESS, p_init->tun1_maddress);
                                 switch (p_init->tuner1_adc) {
                                 case 1:
@@ -1882,6 +1936,7 @@ static struct dvb_frontend_ops stv0900_ops = {
         .read_ber                       = stv0900_read_ber,
         .read_signal_strength           = stv0900_read_signal_strength,
         .read_snr                       = stv0900_read_snr,
+        .read_ucblocks                  = stv0900_read_ucblocks,
 };
 
 struct dvb_frontend *stv0900_attach(const struct stv0900_config *config,
@@ -1915,6 +1970,7 @@ struct dvb_frontend *stv0900_attach(const struct stv0900_config *config,
                 init_params.tun1_iq_inversion   = STV0900_IQ_NORMAL;
                 init_params.tuner1_adc          = config->tun1_adc;
                 init_params.path2_ts_clock      = config->path2_mode;
+                init_params.ts_config           = config->ts_config_regs;
                 init_params.tun2_maddress       = config->tun2_maddress;
                 init_params.tuner2_adc          = config->tun2_adc;
                 init_params.tun2_iq_inversion   = STV0900_IQ_SWAPPED;
diff --git a/drivers/media/dvb/frontends/stv0900_priv.h b/drivers/media/dvb/frontends/stv0900_priv.h
index 67dc8ec634e2..5ed7a145c7d3 100644
--- a/drivers/media/dvb/frontends/stv0900_priv.h
+++ b/drivers/media/dvb/frontends/stv0900_priv.h
@@ -271,6 +271,7 @@ struct stv0900_init_params{
 
         /* IQ from the tuner2 to the demod */
         enum stv0900_iq_inversion       tun2_iq_inversion;
+        struct stv0900_reg              *ts_config;
 };
 
 struct stv0900_search_params {
@@ -363,6 +364,7 @@ struct stv0900_internal{
         u8                      i2c_addr;
         u8                      clkmode;/* 0 for CLKI, 2 for XTALI */
         u8                      chip_id;
+        struct stv0900_reg      *ts_config;
         enum fe_stv0900_error   errs;
         int dmds_used;
 };
diff --git a/drivers/media/dvb/frontends/stv090x.c b/drivers/media/dvb/frontends/stv090x.c
index 96ef745a2e4e..488bdfb34fb3 100644
--- a/drivers/media/dvb/frontends/stv090x.c
+++ b/drivers/media/dvb/frontends/stv090x.c
@@ -2674,7 +2674,7 @@ static u8 stv090x_optimize_carloop(struct stv090x_state *state, enum stv090x_mod
 
 static u8 stv090x_optimize_carloop_short(struct stv090x_state *state)
 {
-        struct stv090x_short_frame_crloop *short_crl;
+        struct stv090x_short_frame_crloop *short_crl = NULL;
         s32 index = 0;
         u8 aclc = 0x0b;
 
@@ -2694,10 +2694,13 @@ static u8 stv090x_optimize_carloop_short(struct stv090x_state *state)
                 break;
         }
 
-        if (state->dev_ver >= 0x30)
-                short_crl = stv090x_s2_short_crl_cut20;
-        else if (state->dev_ver >= 0x20)
+        if (state->dev_ver >= 0x30) {
+                /* Cut 3.0 and up */
                 short_crl = stv090x_s2_short_crl_cut30;
+        } else {
+                /* Cut 2.0 and up: we don't support cuts older than 2.0 */
+                short_crl = stv090x_s2_short_crl_cut20;
+        }
 
         if (state->srate <= 3000000)
                 aclc = short_crl[index].crl_2;
diff --git a/drivers/media/dvb/frontends/tda10048.c b/drivers/media/dvb/frontends/tda10048.c
index 4302c563a6b8..cc8862ce4aae 100644
--- a/drivers/media/dvb/frontends/tda10048.c
+++ b/drivers/media/dvb/frontends/tda10048.c
@@ -210,6 +210,7 @@ static struct pll_tab {
         { TDA10048_CLK_4000,  TDA10048_IF_36130, 10, 0, 0 },
         { TDA10048_CLK_16000, TDA10048_IF_3300,  10, 3, 0 },
         { TDA10048_CLK_16000, TDA10048_IF_3500,  10, 3, 0 },
+        { TDA10048_CLK_16000, TDA10048_IF_3800,  10, 3, 0 },
         { TDA10048_CLK_16000, TDA10048_IF_4000,  10, 3, 0 },
         { TDA10048_CLK_16000, TDA10048_IF_4300,  10, 3, 0 },
         { TDA10048_CLK_16000, TDA10048_IF_36130, 10, 3, 0 },
diff --git a/drivers/media/dvb/siano/smscoreapi.c b/drivers/media/dvb/siano/smscoreapi.c
index 32be382f0e97..a246903c3341 100644
--- a/drivers/media/dvb/siano/smscoreapi.c
+++ b/drivers/media/dvb/siano/smscoreapi.c
@@ -1422,8 +1422,8 @@ int smscore_gpio_configure(struct smscore_device_t *coredev, u8 PinNum,
                 struct smscore_gpio_config *pGpioConfig) {
 
         u32 totalLen;
-        u32 TranslatedPinNum;
-        u32 GroupNum;
+        u32 TranslatedPinNum = 0;
+        u32 GroupNum = 0;
         u32 ElectricChar;
         u32 groupCfg;
         void *buffer;
diff --git a/drivers/media/radio/radio-tea5764.c b/drivers/media/radio/radio-tea5764.c
index 393623818ade..3cd76dddb6aa 100644
--- a/drivers/media/radio/radio-tea5764.c
+++ b/drivers/media/radio/radio-tea5764.c
@@ -322,7 +322,9 @@ static int vidioc_g_tuner(struct file *file, void *priv,
         v->rangehigh  = FREQ_MAX * FREQ_MUL;
         v->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO;
         if (r->tunchk & TEA5764_TUNCHK_STEREO)
-                        v->rxsubchans = V4L2_TUNER_SUB_STEREO;
+                v->rxsubchans = V4L2_TUNER_SUB_STEREO;
+        else
+                v->rxsubchans = V4L2_TUNER_SUB_MONO;
         v->audmode = tea5764_get_audout_mode(radio);
         v->signal = TEA5764_TUNCHK_LEVEL(r->tunchk) * 0xffff / 0xf;
         v->afc = TEA5764_TUNCHK_IFCNT(r->tunchk);
diff --git a/drivers/media/video/Kconfig b/drivers/media/video/Kconfig
index 94f440535c64..061e147f6f26 100644
--- a/drivers/media/video/Kconfig
+++ b/drivers/media/video/Kconfig
@@ -866,9 +866,13 @@ config USB_W9968CF
           module will be called w9968cf.
 
 config USB_OV511
-        tristate "USB OV511 Camera support"
+        tristate "USB OV511 Camera support (DEPRECATED)"
         depends on VIDEO_V4L1
         ---help---
+          This driver is DEPRECATED please use the gspca ov519 module
+          instead. Note that for the ov511 / ov518 support of the gspca module
+          you need atleast version 0.6.0 of libv4l.
+
           Say Y here if you want to connect this type of camera to your
           computer's USB port. See <file:Documentation/video4linux/ov511.txt>
           for more information and for a list of supported cameras.
diff --git a/drivers/media/video/cx18/cx18-controls.c b/drivers/media/video/cx18/cx18-controls.c
index 8e35c3aed544..5136df198338 100644
--- a/drivers/media/video/cx18/cx18-controls.c
+++ b/drivers/media/video/cx18/cx18-controls.c
@@ -61,6 +61,8 @@ int cx18_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *qctrl)
 
         switch (qctrl->id) {
         /* Standard V4L2 controls */
+        case V4L2_CID_USER_CLASS:
+                return v4l2_ctrl_query_fill(qctrl, 0, 0, 0, 0);
         case V4L2_CID_BRIGHTNESS:
         case V4L2_CID_HUE:
         case V4L2_CID_SATURATION:
diff --git a/drivers/media/video/cx231xx/cx231xx-avcore.c b/drivers/media/video/cx231xx/cx231xx-avcore.c
index 6a9464079b4c..28f48f41f218 100644
--- a/drivers/media/video/cx231xx/cx231xx-avcore.c
+++ b/drivers/media/video/cx231xx/cx231xx-avcore.c
@@ -1052,22 +1052,13 @@ int cx231xx_set_audio_decoder_input(struct cx231xx *dev,
 /* Set resolution of the video */
 int cx231xx_resolution_set(struct cx231xx *dev)
 {
-        int width, height;
-        u32 hscale, vscale;
-        int status = 0;
-
-        width = dev->width;
-        height = dev->height;
-
-        get_scale(dev, width, height, &hscale, &vscale);
-
         /* set horzontal scale */
-        status = vid_blk_write_word(dev, HSCALE_CTRL, hscale);
+        int status = vid_blk_write_word(dev, HSCALE_CTRL, dev->hscale);
+        if (status)
+                return status;
 
         /* set vertical scale */
-        status = vid_blk_write_word(dev, VSCALE_CTRL, vscale);
-
-        return status;
+        return vid_blk_write_word(dev, VSCALE_CTRL, dev->vscale);
 }
 
 /******************************************************************************
@@ -2055,7 +2046,7 @@ int cx231xx_initialize_stream_xfer(struct cx231xx *dev, u32 media_type)
 
 int cx231xx_capture_start(struct cx231xx *dev, int start, u8 media_type)
 {
-        int rc;
+        int rc = -1;
         u32 ep_mask = -1;
         struct pcb_config *pcb_config;
 
diff --git a/drivers/media/video/cx231xx/cx231xx-video.c b/drivers/media/video/cx231xx/cx231xx-video.c
index a23ae73fe634..609bae6098d3 100644
--- a/drivers/media/video/cx231xx/cx231xx-video.c
+++ b/drivers/media/video/cx231xx/cx231xx-video.c
@@ -893,9 +893,9 @@ static int check_dev(struct cx231xx *dev)
         return 0;
 }
 
-void get_scale(struct cx231xx *dev,
-               unsigned int width, unsigned int height,
-               unsigned int *hscale, unsigned int *vscale)
+static void get_scale(struct cx231xx *dev,
+                      unsigned int width, unsigned int height,
+                      unsigned int *hscale, unsigned int *vscale)
 {
         unsigned int maxw = norm_maxw(dev);
         unsigned int maxh = norm_maxh(dev);
@@ -907,10 +907,6 @@ void get_scale(struct cx231xx *dev,
         *vscale = (((unsigned long)maxh) << 12) / height - 4096L;
         if (*vscale >= 0x4000)
                 *vscale = 0x3fff;
-
-        dev->hscale = *hscale;
-        dev->vscale = *vscale;
-
 }
 
 /* ------------------------------------------------------------------
@@ -955,8 +951,8 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 {
         struct cx231xx_fh *fh = priv;
         struct cx231xx *dev = fh->dev;
-        int width = f->fmt.pix.width;
-        int height = f->fmt.pix.height;
+        unsigned int width = f->fmt.pix.width;
+        unsigned int height = f->fmt.pix.height;
         unsigned int maxw = norm_maxw(dev);
         unsigned int maxh = norm_maxh(dev);
         unsigned int hscale, vscale;
@@ -971,17 +967,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 
         /* width must even because of the YUYV format
            height must be even because of interlacing */
-        height &= 0xfffe;
-        width &= 0xfffe;
-
-        if (unlikely(height < 32))
-                height = 32;
-        if (unlikely(height > maxh))
-                height = maxh;
-        if (unlikely(width < 48))
-                width = 48;
-        if (unlikely(width > maxw))
-                width = maxw;
+        v4l_bound_align_image(&width, 48, maxw, 1, &height, 32, maxh, 1, 0);
 
         get_scale(dev, width, height, &hscale, &vscale);
 
diff --git a/drivers/media/video/cx231xx/cx231xx.h b/drivers/media/video/cx231xx/cx231xx.h
index e38eb2d425f7..a0f823ac6b8d 100644
--- a/drivers/media/video/cx231xx/cx231xx.h
+++ b/drivers/media/video/cx231xx/cx231xx.h
@@ -722,9 +722,6 @@ int cx231xx_set_video_input_mux(struct cx231xx *dev, u8 input);
 int cx231xx_set_decoder_video_input(struct cx231xx *dev, u8 pin_type, u8 input);
 int cx231xx_do_mode_ctrl_overrides(struct cx231xx *dev);
 int cx231xx_set_audio_input(struct cx231xx *dev, u8 input);
-void get_scale(struct cx231xx *dev,
-               unsigned int width, unsigned int height,
-               unsigned int *hscale, unsigned int *vscale);
 
 /* Provided by cx231xx-video.c */
 int cx231xx_register_extension(struct cx231xx_ops *dev);
diff --git a/drivers/media/video/cx2341x.c b/drivers/media/video/cx2341x.c
index 8ded52946334..4c8e95853fa3 100644
--- a/drivers/media/video/cx2341x.c
+++ b/drivers/media/video/cx2341x.c
@@ -500,6 +500,8 @@ int cx2341x_ctrl_query(const struct cx2341x_mpeg_params *params,
         int err;
 
         switch (qctrl->id) {
+        case V4L2_CID_MPEG_CLASS:
+                return v4l2_ctrl_query_fill(qctrl, 0, 0, 0, 0);
         case V4L2_CID_MPEG_STREAM_TYPE:
                 return v4l2_ctrl_query_fill(qctrl,
                                 V4L2_MPEG_STREAM_TYPE_MPEG2_PS,
diff --git a/drivers/media/video/cx23885/cx23885-dvb.c b/drivers/media/video/cx23885/cx23885-dvb.c
index e236df23370e..48a975134ac5 100644
--- a/drivers/media/video/cx23885/cx23885-dvb.c
+++ b/drivers/media/video/cx23885/cx23885-dvb.c
@@ -45,6 +45,7 @@
 #include "dibx000_common.h"
 #include "zl10353.h"
 #include "stv0900.h"
+#include "stv0900_reg.h"
 #include "stv6110.h"
 #include "lnbh24.h"
 #include "cx24116.h"
@@ -242,12 +243,22 @@ static struct tda18271_std_map hauppauge_tda18271_std_map = {
                       .if_lvl = 6, .rfagc_top = 0x37 },
 };
 
+static struct tda18271_std_map hauppauge_hvr1200_tda18271_std_map = {
+        .dvbt_6   = { .if_freq = 3300, .agc_mode = 3, .std = 4,
+                      .if_lvl = 1, .rfagc_top = 0x37, },
+        .dvbt_7   = { .if_freq = 3800, .agc_mode = 3, .std = 5,
+                      .if_lvl = 1, .rfagc_top = 0x37, },
+        .dvbt_8   = { .if_freq = 4300, .agc_mode = 3, .std = 6,
+                      .if_lvl = 1, .rfagc_top = 0x37, },
+};
+
 static struct tda18271_config hauppauge_tda18271_config = {
         .std_map = &hauppauge_tda18271_std_map,
         .gate    = TDA18271_GATE_ANALOG,
 };
 
 static struct tda18271_config hauppauge_hvr1200_tuner_config = {
+        .std_map = &hauppauge_hvr1200_tda18271_std_map,
         .gate    = TDA18271_GATE_ANALOG,
 };
 
@@ -370,13 +381,25 @@ static struct zl10353_config dvico_fusionhdtv_xc3028 = {
         .disable_i2c_gate_ctrl = 1,
 };
 
+static struct stv0900_reg stv0900_ts_regs[] = {
+        { R0900_TSGENERAL, 0x00 },
+        { R0900_P1_TSSPEED, 0x40 },
+        { R0900_P2_TSSPEED, 0x40 },
+        { R0900_P1_TSCFGM, 0xc0 },
+        { R0900_P2_TSCFGM, 0xc0 },
+        { R0900_P1_TSCFGH, 0xe0 },
+        { R0900_P2_TSCFGH, 0xe0 },
+        { R0900_P1_TSCFGL, 0x20 },
+        { R0900_P2_TSCFGL, 0x20 },
+        { 0xffff, 0xff }, /* terminate */
+};
+
 static struct stv0900_config netup_stv0900_config = {
         .demod_address = 0x68,
         .xtal = 27000000,
         .clkmode = 3,/* 0-CLKI, 2-XTALI, else AUTO */
         .diseqc_mode = 2,/* 2/3 PWM */
-        .path1_mode = 2,/*Serial continues clock */
-        .path2_mode = 2,/*Serial continues clock */
+        .ts_config_regs = stv0900_ts_regs,
         .tun1_maddress = 0,/* 0x60 */
         .tun2_maddress = 3,/* 0x63 */
         .tun1_adc = 1,/* 1 Vpp */
@@ -736,7 +759,8 @@ static int dvb_register(struct cx23885_tsport *port)
                                         if (!dvb_attach(lnbh24_attach,
                                                         fe0->dvb.frontend,
                                                         &i2c_bus->i2c_adap,
-                                                        LNBH24_PCL, 0, 0x09))
+                                                        LNBH24_PCL,
+                                                        LNBH24_TTX, 0x09))
                                                 printk(KERN_ERR
                                                         "No LNBH24 found!\n");
 
@@ -756,7 +780,8 @@ static int dvb_register(struct cx23885_tsport *port)
                                         if (!dvb_attach(lnbh24_attach,
                                                         fe0->dvb.frontend,
                                                         &i2c_bus->i2c_adap,
-                                                        LNBH24_PCL, 0, 0x0a))
+                                                        LNBH24_PCL,
+                                                        LNBH24_TTX, 0x0a))
                                                 printk(KERN_ERR
                                                         "No LNBH24 found!\n");
 
diff --git a/drivers/media/video/cx23885/cx23885-video.c b/drivers/media/video/cx23885/cx23885-video.c
index 66bbd2e71105..70836af3ab48 100644
--- a/drivers/media/video/cx23885/cx23885-video.c
+++ b/drivers/media/video/cx23885/cx23885-video.c
@@ -963,15 +963,8 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
         }
 
         f->fmt.pix.field = field;
-        if (f->fmt.pix.height < 32)
-                f->fmt.pix.height = 32;
-        if (f->fmt.pix.height > maxh)
-                f->fmt.pix.height = maxh;
-        if (f->fmt.pix.width < 48)
-                f->fmt.pix.width = 48;
-        if (f->fmt.pix.width > maxw)
-                f->fmt.pix.width = maxw;
-        f->fmt.pix.width &= ~0x03;
+        v4l_bound_align_image(&f->fmt.pix.width, 48, maxw, 2,
+                              &f->fmt.pix.height, 32, maxh, 0, 0);
         f->fmt.pix.bytesperline =
                 (f->fmt.pix.width * fmt->depth) >> 3;
         f->fmt.pix.sizeimage =
diff --git a/drivers/media/video/cx88/cx88-cards.c b/drivers/media/video/cx88/cx88-cards.c
index 94b7a52629d0..a5cc1c1fc2d6 100644
--- a/drivers/media/video/cx88/cx88-cards.c
+++ b/drivers/media/video/cx88/cx88-cards.c
@@ -1524,33 +1524,45 @@ static const struct cx88_board cx88_boards[] = {
                 },
                 .mpeg           = CX88_MPEG_DVB,
         },
+        /* Terry Wu <terrywu2009@gmail.com> */
+        /* TV Audio :      set GPIO 2, 18, 19 value to 0, 1, 0 */
+        /* FM Audio :      set GPIO 2, 18, 19 value to 0, 0, 0 */
+        /* Line-in Audio : set GPIO 2, 18, 19 value to 0, 1, 1 */
+        /* Mute Audio :    set GPIO 2 value to 1               */
         [CX88_BOARD_WINFAST_TV2000_XP_GLOBAL] = {
-                .name           = "Winfast TV2000 XP Global",
+                .name           = "Leadtek TV2000 XP Global",
                 .tuner_type     = TUNER_XC2028,
                 .tuner_addr     = 0x61,
+                .radio_type     = TUNER_XC2028,
+                .radio_addr     = 0x61,
                 .input          = { {
                         .type   = CX88_VMUX_TELEVISION,
                         .vmux   = 0,
-                        .gpio0  = 0x0400, /* pin 2:mute = 0 (off?) */
+                        .gpio0  = 0x0400,       /* pin 2 = 0 */
                         .gpio1  = 0x0000,
-                        .gpio2  = 0x0800, /* pin 19:audio = 0 (tv) */
-
+                        .gpio2  = 0x0C04,       /* pin 18 = 1, pin 19 = 0 */
+                        .gpio3  = 0x0000,
                 }, {
                         .type   = CX88_VMUX_COMPOSITE1,
                         .vmux   = 1,
-                        .gpio0  = 0x0400, /* probably?  or 0x0404 to turn mute on */
+                        .gpio0  = 0x0400,       /* pin 2 = 0 */
                         .gpio1  = 0x0000,
-                        .gpio2  = 0x0808, /* pin 19:audio = 1 (line) */
-
+                        .gpio2  = 0x0C0C,       /* pin 18 = 1, pin 19 = 1 */
+                        .gpio3  = 0x0000,
                 }, {
                         .type   = CX88_VMUX_SVIDEO,
                         .vmux   = 2,
+                        .gpio0  = 0x0400,       /* pin 2 = 0 */
+                        .gpio1  = 0x0000,
+                        .gpio2  = 0x0C0C,       /* pin 18 = 1, pin 19 = 1 */
+                        .gpio3  = 0x0000,
                 } },
                 .radio = {
                         .type   = CX88_RADIO,
-                        .gpio0  = 0x004ff,
-                        .gpio1  = 0x010ff,
-                        .gpio2  = 0x0ff,
+                        .gpio0  = 0x0400,        /* pin 2 = 0 */
+                        .gpio1  = 0x0000,
+                        .gpio2  = 0x0C00,       /* pin 18 = 0, pin 19 = 0 */
+                        .gpio3  = 0x0000,
                 },
         },
         [CX88_BOARD_POWERCOLOR_REAL_ANGEL] = {
@@ -2438,6 +2450,41 @@ static const struct cx88_subid cx88_subids[] = {
                 .subvendor = 0x107d,
                 .subdevice = 0x6654,
                 .card      = CX88_BOARD_WINFAST_DTV1800H,
+        }, {
+                /* PVR2000 PAL Model [107d:6630] */
+                .subvendor = 0x107d,
+                .subdevice = 0x6630,
+                .card      = CX88_BOARD_LEADTEK_PVR2000,
+        }, {
+                /* PVR2000 PAL Model [107d:6638] */
+                .subvendor = 0x107d,
+                .subdevice = 0x6638,
+                .card      = CX88_BOARD_LEADTEK_PVR2000,
+        }, {
+                /* PVR2000 NTSC Model [107d:6631] */
+                .subvendor = 0x107d,
+                .subdevice = 0x6631,
+                .card      = CX88_BOARD_LEADTEK_PVR2000,
+        }, {
+                /* PVR2000 NTSC Model [107d:6637] */
+                .subvendor = 0x107d,
+                .subdevice = 0x6637,
+                .card      = CX88_BOARD_LEADTEK_PVR2000,
+        }, {
+                /* PVR2000 NTSC Model [107d:663d] */
+                .subvendor = 0x107d,
+                .subdevice = 0x663d,
+                .card      = CX88_BOARD_LEADTEK_PVR2000,
+        }, {
+                /* DV2000 NTSC Model [107d:6621] */
+                .subvendor = 0x107d,
+                .subdevice = 0x6621,
+                .card      = CX88_BOARD_WINFAST_DV2000,
+        }, {
+                /* TV2000 XP Global [107d:6618]  */
+                .subvendor = 0x107d,
+                .subdevice = 0x6618,
+                .card      = CX88_BOARD_WINFAST_TV2000_XP_GLOBAL,
         },
 };
 
@@ -2446,12 +2493,6 @@ static const struct cx88_subid cx88_subids[] = {
 
 static void leadtek_eeprom(struct cx88_core *core, u8 *eeprom_data)
 {
-        /* This is just for the "Winfast 2000XP Expert" board ATM; I don't have data on
-         * any others.
-         *
-         * Byte 0 is 1 on the NTSC board.
-         */
-
         if (eeprom_data[4] != 0x7d ||
             eeprom_data[5] != 0x10 ||
             eeprom_data[7] != 0x66) {
@@ -2459,8 +2500,19 @@ static void leadtek_eeprom(struct cx88_core *core, u8 *eeprom_data)
                 return;
         }
 
-        core->board.tuner_type = (eeprom_data[6] == 0x13) ?
-                TUNER_PHILIPS_FM1236_MK3 : TUNER_PHILIPS_FM1216ME_MK3;
+        /* Terry Wu <terrywu2009@gmail.com> */
+        switch (eeprom_data[6]) {
+        case 0x13: /* SSID 6613 for TV2000 XP Expert NTSC Model */
+        case 0x21: /* SSID 6621 for DV2000 NTSC Model */
+        case 0x31: /* SSID 6631 for PVR2000 NTSC Model */
+        case 0x37: /* SSID 6637 for PVR2000 NTSC Model */
+        case 0x3d: /* SSID 6637 for PVR2000 NTSC Model */
+                core->board.tuner_type = TUNER_PHILIPS_FM1236_MK3;
+                break;
+        default:
+                core->board.tuner_type = TUNER_PHILIPS_FM1216ME_MK3;
+                break;
+        }
 
         info_printk(core, "Leadtek Winfast 2000XP Expert config: "
                     "tuner=%d, eeprom[0]=0x%02x\n",
@@ -2713,7 +2765,6 @@ static int cx88_xc2028_tuner_callback(struct cx88_core *core,
 {
         /* Board-specific callbacks */
         switch (core->boardnr) {
-        case CX88_BOARD_WINFAST_TV2000_XP_GLOBAL:
         case CX88_BOARD_POWERCOLOR_REAL_ANGEL:
         case CX88_BOARD_GENIATECH_X8000_MT:
         case CX88_BOARD_KWORLD_ATSC_120:
@@ -2725,6 +2776,7 @@ static int cx88_xc2028_tuner_callback(struct cx88_core *core,
         case CX88_BOARD_DVICO_FUSIONHDTV_DVB_T_PRO:
         case CX88_BOARD_DVICO_FUSIONHDTV_5_PCI_NANO:
                 return cx88_dvico_xc2028_callback(core, command, arg);
+        case CX88_BOARD_WINFAST_TV2000_XP_GLOBAL:
         case CX88_BOARD_WINFAST_DTV1800H:
                 return cx88_xc3028_winfast1800h_callback(core, command, arg);
         }
@@ -2914,6 +2966,7 @@ static void cx88_card_setup_pre_i2c(struct cx88_core *core)
                 udelay(1000);
                 break;
 
+        case CX88_BOARD_WINFAST_TV2000_XP_GLOBAL:
         case CX88_BOARD_WINFAST_DTV1800H:
                 /* GPIO 12 (xc3028 tuner reset) */
                 cx_set(MO_GP1_IO, 0x1010);
@@ -2950,6 +3003,7 @@ void cx88_setup_xc3028(struct cx88_core *core, struct xc2028_ctrl *ctl)
         case CX88_BOARD_DVICO_FUSIONHDTV_5_PCI_NANO:
                 ctl->demod = XC3028_FE_OREN538;
                 break;
+        case CX88_BOARD_WINFAST_TV2000_XP_GLOBAL:
         case CX88_BOARD_PROLINK_PV_GLOBAL_XTREME:
         case CX88_BOARD_PROLINK_PV_8000GT:
                 /*
@@ -2993,6 +3047,8 @@ static void cx88_card_setup(struct cx88_core *core)
                 if (0 == core->i2c_rc)
                         gdi_eeprom(core, eeprom);
                 break;
+        case CX88_BOARD_LEADTEK_PVR2000:
+        case CX88_BOARD_WINFAST_DV2000:
         case CX88_BOARD_WINFAST2000XP_EXPERT:
                 if (0 == core->i2c_rc)
                         leadtek_eeprom(core, eeprom);
diff --git a/drivers/media/video/cx88/cx88-video.c b/drivers/media/video/cx88/cx88-video.c
index 0ccac702bea4..b12770848c00 100644
--- a/drivers/media/video/cx88/cx88-video.c
+++ b/drivers/media/video/cx88/cx88-video.c
@@ -1111,15 +1111,8 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
         }
 
         f->fmt.pix.field = field;
-        if (f->fmt.pix.height < 32)
-                f->fmt.pix.height = 32;
-        if (f->fmt.pix.height > maxh)
-                f->fmt.pix.height = maxh;
-        if (f->fmt.pix.width < 48)
-                f->fmt.pix.width = 48;
-        if (f->fmt.pix.width > maxw)
-                f->fmt.pix.width = maxw;
-        f->fmt.pix.width &= ~0x03;
+        v4l_bound_align_image(&f->fmt.pix.width, 48, maxw, 2,
+                              &f->fmt.pix.height, 32, maxh, 0, 0);
         f->fmt.pix.bytesperline =
                 (f->fmt.pix.width * fmt->depth) >> 3;
         f->fmt.pix.sizeimage =
diff --git a/drivers/media/video/em28xx/em28xx-cards.c b/drivers/media/video/em28xx/em28xx-cards.c
index 00cc791a9e44..c43fdb9bc888 100644
--- a/drivers/media/video/em28xx/em28xx-cards.c
+++ b/drivers/media/video/em28xx/em28xx-cards.c
@@ -139,6 +139,24 @@ static struct em28xx_reg_seq kworld_330u_digital[] = {
         { -1,                   -1,     -1,             -1},
 };
 
+/* Evga inDtube
+   GPIO0 - Enable digital power (s5h1409) - low to enable
+   GPIO1 - Enable analog power (tvp5150/emp202) - low to enable
+   GPIO4 - xc3028 reset
+   GOP3  - s5h1409 reset
+ */
+static struct em28xx_reg_seq evga_indtube_analog[] = {
+        {EM28XX_R08_GPIO,       0x79,   0xff,           60},
+        {       -1,             -1,     -1,             -1},
+};
+
+static struct em28xx_reg_seq evga_indtube_digital[] = {
+        {EM28XX_R08_GPIO,       0x7a,   0xff,            1},
+        {EM2880_R04_GPO,        0x04,   0xff,           10},
+        {EM2880_R04_GPO,        0x0c,   0xff,            1},
+        { -1,                   -1,     -1,             -1},
+};
+
 /* Callback for the most boards */
 static struct em28xx_reg_seq default_tuner_gpio[] = {
         {EM28XX_R08_GPIO,       EM_GPIO_4,      EM_GPIO_4,      10},
@@ -1449,6 +1467,33 @@ struct em28xx_board em28xx_boards[] = {
                         .gpio     = terratec_av350_unmute_gpio,
                 } },
         },
+        [EM2882_BOARD_EVGA_INDTUBE] = {
+                .name         = "Evga inDtube",
+                .tuner_type   = TUNER_XC2028,
+                .tuner_gpio   = default_tuner_gpio,
+                .decoder      = EM28XX_TVP5150,
+                .xclk         = EM28XX_XCLK_FREQUENCY_12MHZ, /* NEC IR */
+                .mts_firmware = 1,
+                .has_dvb      = 1,
+                .dvb_gpio     = evga_indtube_digital,
+                .ir_codes     = ir_codes_evga_indtube,
+                .input        = { {
+                        .type     = EM28XX_VMUX_TELEVISION,
+                        .vmux     = TVP5150_COMPOSITE0,
+                        .amux     = EM28XX_AMUX_VIDEO,
+                        .gpio     = evga_indtube_analog,
+                }, {
+                        .type     = EM28XX_VMUX_COMPOSITE1,
+                        .vmux     = TVP5150_COMPOSITE1,
+                        .amux     = EM28XX_AMUX_LINE_IN,
+                        .gpio     = evga_indtube_analog,
+                }, {
+                        .type     = EM28XX_VMUX_SVIDEO,
+                        .vmux     = TVP5150_SVIDEO,
+                        .amux     = EM28XX_AMUX_LINE_IN,
+                        .gpio     = evga_indtube_analog,
+                } },
+        },
 };
 const unsigned int em28xx_bcount = ARRAY_SIZE(em28xx_boards);
 
@@ -1571,6 +1616,7 @@ static struct em28xx_hash_table em28xx_eeprom_hash[] = {
         {0x72cc5a8b, EM2820_BOARD_PROLINK_PLAYTV_BOX4_USB2, TUNER_YMEC_TVF_5533MF},
         {0x966a0441, EM2880_BOARD_KWORLD_DVB_310U, TUNER_XC2028},
         {0x9567eb1a, EM2880_BOARD_EMPIRE_DUAL_TV, TUNER_XC2028},
+        {0xcee44a99, EM2882_BOARD_EVGA_INDTUBE, TUNER_XC2028},
 };
 
 /* I2C devicelist hash table for devices with generic USB IDs */
@@ -1834,6 +1880,10 @@ static void em28xx_setup_xc3028(struct em28xx *dev, struct xc2028_ctrl *ctl)
                 ctl->demod = XC3028_FE_CHINA;
                 ctl->fname = XC2028_DEFAULT_FIRMWARE;
                 break;
+        case EM2882_BOARD_EVGA_INDTUBE:
+                ctl->demod = XC3028_FE_CHINA;
+                ctl->fname = XC3028L_DEFAULT_FIRMWARE;
+                break;
         default:
                 ctl->demod = XC3028_FE_OREN538;
         }
@@ -2101,6 +2151,12 @@ void em28xx_card_setup(struct em28xx *dev)
         case EM2880_BOARD_MSI_DIGIVOX_AD:
                 if (!em28xx_hint_board(dev))
                         em28xx_set_model(dev);
+
+                /* In cases where we had to use a board hint, the call to
+                   em28xx_set_mode() in em28xx_pre_card_setup() was a no-op,
+                   so make the call now so the analog GPIOs are set properly
+                   before probing the i2c bus. */
+                em28xx_set_mode(dev, EM28XX_ANALOG_MODE);
                 break;
         }
 
diff --git a/drivers/media/video/em28xx/em28xx-dvb.c b/drivers/media/video/em28xx/em28xx-dvb.c
index 563dd2b1c8e9..e7b47c8da8f3 100644
--- a/drivers/media/video/em28xx/em28xx-dvb.c
+++ b/drivers/media/video/em28xx/em28xx-dvb.c
@@ -445,6 +445,7 @@ static int dvb_init(struct em28xx *dev)
                 }
                 break;
         case EM2883_BOARD_KWORLD_HYBRID_330U:
+        case EM2882_BOARD_EVGA_INDTUBE:
                 dvb->frontend = dvb_attach(s5h1409_attach,
                                            &em28xx_s5h1409_with_xc3028,
                                            &dev->i2c_adap);
diff --git a/drivers/media/video/em28xx/em28xx-video.c b/drivers/media/video/em28xx/em28xx-video.c
index 882796e84dbc..8fe1beecfffa 100644
--- a/drivers/media/video/em28xx/em28xx-video.c
+++ b/drivers/media/video/em28xx/em28xx-video.c
@@ -687,8 +687,8 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 {
         struct em28xx_fh      *fh    = priv;
         struct em28xx         *dev   = fh->dev;
-        int                   width  = f->fmt.pix.width;
-        int                   height = f->fmt.pix.height;
+        unsigned int          width  = f->fmt.pix.width;
+        unsigned int          height = f->fmt.pix.height;
         unsigned int          maxw   = norm_maxw(dev);
         unsigned int          maxh   = norm_maxh(dev);
         unsigned int          hscale, vscale;
@@ -701,34 +701,20 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
                 return -EINVAL;
         }
 
-        /* width must even because of the YUYV format
-           height must be even because of interlacing */
-        height &= 0xfffe;
-        width  &= 0xfffe;
-
-        if (unlikely(height < 32))
-                height = 32;
-        if (unlikely(height > maxh))
-                height = maxh;
-        if (unlikely(width < 48))
-                width = 48;
-        if (unlikely(width > maxw))
-                width = maxw;
-
         if (dev->board.is_em2800) {
                 /* the em2800 can only scale down to 50% */
-                if (height % (maxh / 2))
-                        height = maxh;
-                if (width % (maxw / 2))
-                        width = maxw;
-                /* according to empiatech support */
-                /* the MaxPacketSize is to small to support */
-                /* framesizes larger than 640x480 @ 30 fps */
-                /* or 640x576 @ 25 fps. As this would cut */
-                /* of a part of the image we prefer */
-                /* 360x576 or 360x480 for now */
+                height = height > (3 * maxh / 4) ? maxh : maxh / 2;
+                width = width > (3 * maxw / 4) ? maxw : maxw / 2;
+                /* According to empiatech support the MaxPacketSize is too small
+                 * to support framesizes larger than 640x480 @ 30 fps or 640x576
+                 * @ 25 fps.  As this would cut of a part of the image we prefer
+                 * 360x576 or 360x480 for now */
                 if (width == maxw && height == maxh)
                         width /= 2;
+        } else {
+                /* width must even because of the YUYV format
+                   height must be even because of interlacing */
+                v4l_bound_align_image(&width, 48, maxw, 1, &height, 32, maxh, 1, 0);
         }
 
         get_scale(dev, width, height, &hscale, &vscale);
diff --git a/drivers/media/video/em28xx/em28xx.h b/drivers/media/video/em28xx/em28xx.h
index 8bf81be1da61..813ce45c2f99 100644
--- a/drivers/media/video/em28xx/em28xx.h
+++ b/drivers/media/video/em28xx/em28xx.h
@@ -106,6 +106,7 @@
 #define EM2860_BOARD_TERRATEC_GRABBY              67
 #define EM2860_BOARD_TERRATEC_AV350               68
 #define EM2882_BOARD_KWORLD_ATSC_315U             69
+#define EM2882_BOARD_EVGA_INDTUBE                 70
 
 /* Limits minimum and default number of buffers */
 #define EM28XX_MIN_BUF 4
diff --git a/drivers/media/video/gspca/gspca.c b/drivers/media/video/gspca/gspca.c
index f7e0355ad644..1e89600986c8 100644
--- a/drivers/media/video/gspca/gspca.c
+++ b/drivers/media/video/gspca/gspca.c
@@ -1042,13 +1042,11 @@ static int vidioc_queryctrl(struct file *file, void *priv,
                 for (i = 0; i < gspca_dev->sd_desc->nctrls; i++) {
                         if (gspca_dev->ctrl_dis & (1 << i))
                                 continue;
-                        if (ctrls->qctrl.id < id)
+                        if (gspca_dev->sd_desc->ctrls[i].qctrl.id < id)
                                 continue;
-                        if (ctrls != NULL) {
-                                if (gspca_dev->sd_desc->ctrls[i].qctrl.id
+                        if (ctrls && gspca_dev->sd_desc->ctrls[i].qctrl.id
                                             > ctrls->qctrl.id)
-                                        continue;
-                        }
+                                continue;
                         ctrls = &gspca_dev->sd_desc->ctrls[i];
                 }
         } else {
diff --git a/drivers/media/video/gspca/ov519.c b/drivers/media/video/gspca/ov519.c
index 188866ac6cef..2f6e135d94bc 100644
--- a/drivers/media/video/gspca/ov519.c
+++ b/drivers/media/video/gspca/ov519.c
@@ -50,12 +50,18 @@ static int i2c_detect_tries = 10;
 struct sd {
         struct gspca_dev gspca_dev;             /* !! must be the first item */
 
+        __u8 packet_nr;
+
         char bridge;
 #define BRIDGE_OV511            0
 #define BRIDGE_OV511PLUS        1
 #define BRIDGE_OV518            2
 #define BRIDGE_OV518PLUS        3
 #define BRIDGE_OV519            4
+#define BRIDGE_MASK             7
+
+        char invert_led;
+#define BRIDGE_INVERT_LED       8
 
         /* Determined by sensor type */
         __u8 sif;
@@ -65,22 +71,25 @@ struct sd {
         __u8 colors;
         __u8 hflip;
         __u8 vflip;
+        __u8 autobrightness;
+        __u8 freq;
 
         __u8 stopped;           /* Streaming is temporarily paused */
 
-        __u8 frame_rate;        /* current Framerate (OV519 only) */
-        __u8 clockdiv;          /* clockdiv override for OV519 only */
+        __u8 frame_rate;        /* current Framerate */
+        __u8 clockdiv;          /* clockdiv override */
 
         char sensor;            /* Type of image sensor chip (SEN_*) */
 #define SEN_UNKNOWN 0
 #define SEN_OV6620 1
 #define SEN_OV6630 2
-#define SEN_OV7610 3
-#define SEN_OV7620 4
-#define SEN_OV7640 5
-#define SEN_OV7670 6
-#define SEN_OV76BE 7
-#define SEN_OV8610 8
+#define SEN_OV66308AF 3
+#define SEN_OV7610 4
+#define SEN_OV7620 5
+#define SEN_OV7640 6
+#define SEN_OV7670 7
+#define SEN_OV76BE 8
+#define SEN_OV8610 9
 };
 
 /* V4L2 controls supported by the driver */
@@ -94,11 +103,17 @@ static int sd_sethflip(struct gspca_dev *gspca_dev, __s32 val);
 static int sd_gethflip(struct gspca_dev *gspca_dev, __s32 *val);
 static int sd_setvflip(struct gspca_dev *gspca_dev, __s32 val);
 static int sd_getvflip(struct gspca_dev *gspca_dev, __s32 *val);
+static int sd_setautobrightness(struct gspca_dev *gspca_dev, __s32 val);
+static int sd_getautobrightness(struct gspca_dev *gspca_dev, __s32 *val);
+static int sd_setfreq(struct gspca_dev *gspca_dev, __s32 val);
+static int sd_getfreq(struct gspca_dev *gspca_dev, __s32 *val);
 static void setbrightness(struct gspca_dev *gspca_dev);
 static void setcontrast(struct gspca_dev *gspca_dev);
 static void setcolors(struct gspca_dev *gspca_dev);
+static void setautobrightness(struct sd *sd);
+static void setfreq(struct sd *sd);
 
-static struct ctrl sd_ctrls[] = {
+static const struct ctrl sd_ctrls[] = {
         {
             {
                 .id      = V4L2_CID_BRIGHTNESS,
@@ -141,7 +156,7 @@ static struct ctrl sd_ctrls[] = {
             .set = sd_setcolors,
             .get = sd_getcolors,
         },
-/* next controls work with ov7670 only */
+/* The flip controls work with ov7670 only */
 #define HFLIP_IDX 3
         {
             {
@@ -172,6 +187,51 @@ static struct ctrl sd_ctrls[] = {
             .set = sd_setvflip,
             .get = sd_getvflip,
         },
+#define AUTOBRIGHT_IDX 5
+        {
+            {
+                .id      = V4L2_CID_AUTOBRIGHTNESS,
+                .type    = V4L2_CTRL_TYPE_BOOLEAN,
+                .name    = "Auto Brightness",
+                .minimum = 0,
+                .maximum = 1,
+                .step    = 1,
+#define AUTOBRIGHT_DEF 1
+                .default_value = AUTOBRIGHT_DEF,
+            },
+            .set = sd_setautobrightness,
+            .get = sd_getautobrightness,
+        },
+#define FREQ_IDX 6
+        {
+            {
+                .id      = V4L2_CID_POWER_LINE_FREQUENCY,
+                .type    = V4L2_CTRL_TYPE_MENU,
+                .name    = "Light frequency filter",
+                .minimum = 0,
+                .maximum = 2,   /* 0: 0, 1: 50Hz, 2:60Hz */
+                .step    = 1,
+#define FREQ_DEF 0
+                .default_value = FREQ_DEF,
+            },
+            .set = sd_setfreq,
+            .get = sd_getfreq,
+        },
+#define OV7670_FREQ_IDX 7
+        {
+            {
+                .id      = V4L2_CID_POWER_LINE_FREQUENCY,
+                .type    = V4L2_CTRL_TYPE_MENU,
+                .name    = "Light frequency filter",
+                .minimum = 0,
+                .maximum = 3,   /* 0: 0, 1: 50Hz, 2:60Hz 3: Auto Hz */
+                .step    = 1,
+#define OV7670_FREQ_DEF 3
+                .default_value = OV7670_FREQ_DEF,
+            },
+            .set = sd_setfreq,
+            .get = sd_getfreq,
+        },
 };
 
 static const struct v4l2_pix_format ov519_vga_mode[] = {
@@ -187,11 +247,21 @@ static const struct v4l2_pix_format ov519_vga_mode[] = {
                 .priv = 0},
 };
 static const struct v4l2_pix_format ov519_sif_mode[] = {
+        {160, 120, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
+                .bytesperline = 160,
+                .sizeimage = 160 * 120 * 3 / 8 + 590,
+                .colorspace = V4L2_COLORSPACE_JPEG,
+                .priv = 3},
         {176, 144, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
                 .bytesperline = 176,
                 .sizeimage = 176 * 144 * 3 / 8 + 590,
                 .colorspace = V4L2_COLORSPACE_JPEG,
                 .priv = 1},
+        {320, 240, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
+                .bytesperline = 320,
+                .sizeimage = 320 * 240 * 3 / 8 + 590,
+                .colorspace = V4L2_COLORSPACE_JPEG,
+                .priv = 2},
         {352, 288, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
                 .bytesperline = 352,
                 .sizeimage = 352 * 288 * 3 / 8 + 590,
@@ -199,42 +269,118 @@ static const struct v4l2_pix_format ov519_sif_mode[] = {
                 .priv = 0},
 };
 
+/* Note some of the sizeimage values for the ov511 / ov518 may seem
+   larger then necessary, however they need to be this big as the ov511 /
+   ov518 always fills the entire isoc frame, using 0 padding bytes when
+   it doesn't have any data. So with low framerates the amount of data
+   transfered can become quite large (libv4l will remove all the 0 padding
+   in userspace). */
 static const struct v4l2_pix_format ov518_vga_mode[] = {
         {320, 240, V4L2_PIX_FMT_OV518, V4L2_FIELD_NONE,
                 .bytesperline = 320,
-                .sizeimage = 320 * 240 * 3 / 8 + 590,
+                .sizeimage = 320 * 240 * 3,
                 .colorspace = V4L2_COLORSPACE_JPEG,
                 .priv = 1},
         {640, 480, V4L2_PIX_FMT_OV518, V4L2_FIELD_NONE,
                 .bytesperline = 640,
-                .sizeimage = 640 * 480 * 3 / 8 + 590,
+                .sizeimage = 640 * 480 * 2,
                 .colorspace = V4L2_COLORSPACE_JPEG,
                 .priv = 0},
 };
 static const struct v4l2_pix_format ov518_sif_mode[] = {
+        {160, 120, V4L2_PIX_FMT_OV518, V4L2_FIELD_NONE,
+                .bytesperline = 160,
+                .sizeimage = 70000,
+                .colorspace = V4L2_COLORSPACE_JPEG,
+                .priv = 3},
         {176, 144, V4L2_PIX_FMT_OV518, V4L2_FIELD_NONE,
                 .bytesperline = 176,
-                .sizeimage = 40000,
+                .sizeimage = 70000,
                 .colorspace = V4L2_COLORSPACE_JPEG,
                 .priv = 1},
+        {320, 240, V4L2_PIX_FMT_OV518, V4L2_FIELD_NONE,
+                .bytesperline = 320,
+                .sizeimage = 320 * 240 * 3,
+                .colorspace = V4L2_COLORSPACE_JPEG,
+                .priv = 2},
         {352, 288, V4L2_PIX_FMT_OV518, V4L2_FIELD_NONE,
                 .bytesperline = 352,
-                .sizeimage = 352 * 288 * 3 / 8 + 590,
+                .sizeimage = 352 * 288 * 3,
                 .colorspace = V4L2_COLORSPACE_JPEG,
                 .priv = 0},
 };
 
+static const struct v4l2_pix_format ov511_vga_mode[] = {
+        {320, 240, V4L2_PIX_FMT_OV511, V4L2_FIELD_NONE,
+                .bytesperline = 320,
+                .sizeimage = 320 * 240 * 3,
+                .colorspace = V4L2_COLORSPACE_JPEG,
+                .priv = 1},
+        {640, 480, V4L2_PIX_FMT_OV511, V4L2_FIELD_NONE,
+                .bytesperline = 640,
+                .sizeimage = 640 * 480 * 2,
+                .colorspace = V4L2_COLORSPACE_JPEG,
+                .priv = 0},
+};
+static const struct v4l2_pix_format ov511_sif_mode[] = {
+        {160, 120, V4L2_PIX_FMT_OV511, V4L2_FIELD_NONE,
+                .bytesperline = 160,
+                .sizeimage = 70000,
+                .colorspace = V4L2_COLORSPACE_JPEG,
+                .priv = 3},
+        {176, 144, V4L2_PIX_FMT_OV511, V4L2_FIELD_NONE,
+                .bytesperline = 176,
+                .sizeimage = 70000,
+                .colorspace = V4L2_COLORSPACE_JPEG,
+                .priv = 1},
+        {320, 240, V4L2_PIX_FMT_OV511, V4L2_FIELD_NONE,
+                .bytesperline = 320,
+                .sizeimage = 320 * 240 * 3,
+                .colorspace = V4L2_COLORSPACE_JPEG,
+                .priv = 2},
+        {352, 288, V4L2_PIX_FMT_OV511, V4L2_FIELD_NONE,
+                .bytesperline = 352,
+                .sizeimage = 352 * 288 * 3,
+                .colorspace = V4L2_COLORSPACE_JPEG,
+                .priv = 0},
+};
 
 /* Registers common to OV511 / OV518 */
+#define R51x_FIFO_PSIZE                 0x30    /* 2 bytes wide w/ OV518(+) */
 #define R51x_SYS_RESET                  0x50
+        /* Reset type flags */
+        #define OV511_RESET_OMNICE      0x08
 #define R51x_SYS_INIT                   0x53
 #define R51x_SYS_SNAP                   0x52
 #define R51x_SYS_CUST_ID                0x5F
 #define R51x_COMP_LUT_BEGIN             0x80
 
 /* OV511 Camera interface register numbers */
+#define R511_CAM_DELAY                  0x10
+#define R511_CAM_EDGE                   0x11
+#define R511_CAM_PXCNT                  0x12
+#define R511_CAM_LNCNT                  0x13
+#define R511_CAM_PXDIV                  0x14
+#define R511_CAM_LNDIV                  0x15
+#define R511_CAM_UV_EN                  0x16
+#define R511_CAM_LINE_MODE              0x17
+#define R511_CAM_OPTS                   0x18
+
+#define R511_SNAP_FRAME                 0x19
+#define R511_SNAP_PXCNT                 0x1A
+#define R511_SNAP_LNCNT                 0x1B
+#define R511_SNAP_PXDIV                 0x1C
+#define R511_SNAP_LNDIV                 0x1D
+#define R511_SNAP_UV_EN                 0x1E
+#define R511_SNAP_UV_EN                 0x1E
+#define R511_SNAP_OPTS                  0x1F
+
+#define R511_DRAM_FLOW_CTL              0x20
+#define R511_FIFO_OPTS                  0x31
+#define R511_I2C_CTL                    0x40
 #define R511_SYS_LED_CTL                0x55    /* OV511+ only */
-#define OV511_RESET_NOREGS              0x3F    /* All but OV511 & regs */
+#define R511_COMP_EN                    0x78
+#define R511_COMP_LUT_EN                0x79
 
 /* OV518 Camera interface register numbers */
 #define R518_GPIO_OUT                   0x56    /* OV518(+) only */
@@ -383,7 +529,7 @@ static const struct ov_i2c_regvals norm_6x20[] = {
         { 0x28, 0x05 },
         { 0x2a, 0x04 }, /* Disable framerate adjust */
 /*      { 0x2b, 0xac },  * Framerate; Set 2a[7] first */
-        { 0x2d, 0x99 },
+        { 0x2d, 0x85 },
         { 0x33, 0xa0 }, /* Color Processing Parameter */
         { 0x34, 0xd2 }, /* Max A/D range */
         { 0x38, 0x8b },
@@ -416,7 +562,7 @@ static const struct ov_i2c_regvals norm_6x30[] = {
         { 0x07, 0x2d }, /* Sharpness */
         { 0x0c, 0x20 },
         { 0x0d, 0x20 },
-        { 0x0e, 0x20 },
+        { 0x0e, 0xa0 }, /* Was 0x20, bit7 enables a 2x gain which we need */
         { 0x0f, 0x05 },
         { 0x10, 0x9a },
         { 0x11, 0x00 }, /* Pixel clock = fastest */
@@ -558,7 +704,7 @@ static const struct ov_i2c_regvals norm_7620[] = {
         { 0x23, 0x00 },
         { 0x26, 0xa2 },
         { 0x27, 0xea },
-        { 0x28, 0x20 },
+        { 0x28, 0x22 }, /* Was 0x20, bit1 enables a 2x gain which we need */
         { 0x29, 0x00 },
         { 0x2a, 0x10 },
         { 0x2b, 0x00 },
@@ -999,13 +1145,128 @@ static int ov518_reg_w32(struct sd *sd, __u16 index, u32 value, int n)
         return ret;
 }
 
+static int ov511_i2c_w(struct sd *sd, __u8 reg, __u8 value)
+{
+        int rc, retries;
+
+        PDEBUG(D_USBO, "i2c 0x%02x -> [0x%02x]", value, reg);
+
+        /* Three byte write cycle */
+        for (retries = 6; ; ) {
+                /* Select camera register */
+                rc = reg_w(sd, R51x_I2C_SADDR_3, reg);
+                if (rc < 0)
+                        return rc;
+
+                /* Write "value" to I2C data port of OV511 */
+                rc = reg_w(sd, R51x_I2C_DATA, value);
+                if (rc < 0)
+                        return rc;
+
+                /* Initiate 3-byte write cycle */
+                rc = reg_w(sd, R511_I2C_CTL, 0x01);
+                if (rc < 0)
+                        return rc;
+
+                do
+                        rc = reg_r(sd, R511_I2C_CTL);
+                while (rc > 0 && ((rc & 1) == 0)); /* Retry until idle */
+
+                if (rc < 0)
+                        return rc;
+
+                if ((rc & 2) == 0) /* Ack? */
+                        break;
+                if (--retries < 0) {
+                        PDEBUG(D_USBO, "i2c write retries exhausted");
+                        return -1;
+                }
+        }
+
+        return 0;
+}
+
+static int ov511_i2c_r(struct sd *sd, __u8 reg)
+{
+        int rc, value, retries;
+
+        /* Two byte write cycle */
+        for (retries = 6; ; ) {
+                /* Select camera register */
+                rc = reg_w(sd, R51x_I2C_SADDR_2, reg);
+                if (rc < 0)
+                        return rc;
+
+                /* Initiate 2-byte write cycle */
+                rc = reg_w(sd, R511_I2C_CTL, 0x03);
+                if (rc < 0)
+                        return rc;
+
+                do
+                        rc = reg_r(sd, R511_I2C_CTL);
+                while (rc > 0 && ((rc & 1) == 0)); /* Retry until idle */
+
+                if (rc < 0)
+                        return rc;
+
+                if ((rc & 2) == 0) /* Ack? */
+                        break;
+
+                /* I2C abort */
+                reg_w(sd, R511_I2C_CTL, 0x10);
+
+                if (--retries < 0) {
+                        PDEBUG(D_USBI, "i2c write retries exhausted");
+                        return -1;
+                }
+        }
+
+        /* Two byte read cycle */
+        for (retries = 6; ; ) {
+                /* Initiate 2-byte read cycle */
+                rc = reg_w(sd, R511_I2C_CTL, 0x05);
+                if (rc < 0)
+                        return rc;
+
+                do
+                        rc = reg_r(sd, R511_I2C_CTL);
+                while (rc > 0 && ((rc & 1) == 0)); /* Retry until idle */
+
+                if (rc < 0)
+                        return rc;
+
+                if ((rc & 2) == 0) /* Ack? */
+                        break;
+
+                /* I2C abort */
+                rc = reg_w(sd, R511_I2C_CTL, 0x10);
+                if (rc < 0)
+                        return rc;
+
+                if (--retries < 0) {
+                        PDEBUG(D_USBI, "i2c read retries exhausted");
+                        return -1;
+                }
+        }
+
+        value = reg_r(sd, R51x_I2C_DATA);
+
+        PDEBUG(D_USBI, "i2c [0x%02X] -> 0x%02X", reg, value);
+
+        /* This is needed to make i2c_w() work */
+        rc = reg_w(sd, R511_I2C_CTL, 0x05);
+        if (rc < 0)
+                return rc;
+
+        return value;
+}
 
 /*
  * The OV518 I2C I/O procedure is different, hence, this function.
  * This is normally only called from i2c_w(). Note that this function
  * always succeeds regardless of whether the sensor is present and working.
  */
-static int i2c_w(struct sd *sd,
+static int ov518_i2c_w(struct sd *sd,
                 __u8 reg,
                 __u8 value)
 {
@@ -1040,7 +1301,7 @@ static int i2c_w(struct sd *sd,
  * This is normally only called from i2c_r(). Note that this function
  * always succeeds regardless of whether the sensor is present and working.
  */
-static int i2c_r(struct sd *sd, __u8 reg)
+static int ov518_i2c_r(struct sd *sd, __u8 reg)
 {
         int rc, value;
 
@@ -1063,6 +1324,34 @@ static int i2c_r(struct sd *sd, __u8 reg)
         return value;
 }
 
+static int i2c_w(struct sd *sd, __u8 reg, __u8 value)
+{
+        switch (sd->bridge) {
+        case BRIDGE_OV511:
+        case BRIDGE_OV511PLUS:
+                return ov511_i2c_w(sd, reg, value);
+        case BRIDGE_OV518:
+        case BRIDGE_OV518PLUS:
+        case BRIDGE_OV519:
+                return ov518_i2c_w(sd, reg, value);
+        }
+        return -1; /* Should never happen */
+}
+
+static int i2c_r(struct sd *sd, __u8 reg)
+{
+        switch (sd->bridge) {
+        case BRIDGE_OV511:
+        case BRIDGE_OV511PLUS:
+                return ov511_i2c_r(sd, reg);
+        case BRIDGE_OV518:
+        case BRIDGE_OV518PLUS:
+        case BRIDGE_OV519:
+                return ov518_i2c_r(sd, reg);
+        }
+        return -1; /* Should never happen */
+}
+
 /* Writes bits at positions specified by mask to an I2C reg. Bits that are in
  * the same position as 1's in "mask" are cleared and set to "value". Bits
  * that are in the same position as 0's in "mask" are preserved, regardless
@@ -1242,7 +1531,6 @@ static int ov8xx0_configure(struct sd *sd)
         }
 
         /* Set sensor-specific vars */
-/*      sd->sif = 0;            already done */
         return 0;
 }
 
@@ -1279,15 +1567,13 @@ static int ov7xx0_configure(struct sd *sd)
                 }
         } else if ((rc & 3) == 1) {
                 /* I don't know what's different about the 76BE yet. */
-                if (i2c_r(sd, 0x15) & 1)
+                if (i2c_r(sd, 0x15) & 1) {
                         PDEBUG(D_PROBE, "Sensor is an OV7620AE");
-                else
+                        sd->sensor = SEN_OV7620;
+                } else {
                         PDEBUG(D_PROBE, "Sensor is an OV76BE");
-
-                /* OV511+ will return all zero isoc data unless we
-                 * configure the sensor as a 7620. Someone needs to
-                 * find the exact reg. setting that causes this. */
-                sd->sensor = SEN_OV76BE;
+                        sd->sensor = SEN_OV76BE;
+                }
         } else if ((rc & 3) == 0) {
                 /* try to read product id registers */
                 high = i2c_r(sd, 0x0a);
@@ -1333,7 +1619,6 @@ static int ov7xx0_configure(struct sd *sd)
         }
 
         /* Set sensor-specific vars */
-/*      sd->sif = 0;            already done */
         return 0;
 }
 
@@ -1362,13 +1647,14 @@ static int ov6xx0_configure(struct sd *sd)
                 break;
         case 0x01:
                 sd->sensor = SEN_OV6620;
+                PDEBUG(D_PROBE, "Sensor is an OV6620");
                 break;
         case 0x02:
                 sd->sensor = SEN_OV6630;
                 PDEBUG(D_PROBE, "Sensor is an OV66308AE");
                 break;
         case 0x03:
-                sd->sensor = SEN_OV6630;
+                sd->sensor = SEN_OV66308AF;
                 PDEBUG(D_PROBE, "Sensor is an OV66308AF");
                 break;
         case 0x90:
@@ -1391,6 +1677,9 @@ static int ov6xx0_configure(struct sd *sd)
 /* Turns on or off the LED. Only has an effect with OV511+/OV518(+)/OV519 */
 static void ov51x_led_control(struct sd *sd, int on)
 {
+        if (sd->invert_led)
+                on = !on;
+
         switch (sd->bridge) {
         /* OV511 has no LED control */
         case BRIDGE_OV511PLUS:
@@ -1406,9 +1695,31 @@ static void ov51x_led_control(struct sd *sd, int on)
         }
 }
 
-/* OV518 quantization tables are 8x4 (instead of 8x8) */
-static int ov518_upload_quan_tables(struct sd *sd)
+static int ov51x_upload_quan_tables(struct sd *sd)
 {
+        const unsigned char yQuanTable511[] = {
+                0, 1, 1, 2, 2, 3, 3, 4,
+                1, 1, 1, 2, 2, 3, 4, 4,
+                1, 1, 2, 2, 3, 4, 4, 4,
+                2, 2, 2, 3, 4, 4, 4, 4,
+                2, 2, 3, 4, 4, 5, 5, 5,
+                3, 3, 4, 4, 5, 5, 5, 5,
+                3, 4, 4, 4, 5, 5, 5, 5,
+                4, 4, 4, 4, 5, 5, 5, 5
+        };
+
+        const unsigned char uvQuanTable511[] = {
+                0, 2, 2, 3, 4, 4, 4, 4,
+                2, 2, 2, 4, 4, 4, 4, 4,
+                2, 2, 3, 4, 4, 4, 4, 4,
+                3, 4, 4, 4, 4, 4, 4, 4,
+                4, 4, 4, 4, 4, 4, 4, 4,
+                4, 4, 4, 4, 4, 4, 4, 4,
+                4, 4, 4, 4, 4, 4, 4, 4,
+                4, 4, 4, 4, 4, 4, 4, 4
+        };
+
+        /* OV518 quantization tables are 8x4 (instead of 8x8) */
         const unsigned char yQuanTable518[] = {
                 5, 4, 5, 6, 6, 7, 7, 7,
                 5, 5, 5, 5, 6, 7, 7, 7,
@@ -1423,14 +1734,23 @@ static int ov518_upload_quan_tables(struct sd *sd)
                 7, 7, 7, 7, 7, 7, 8, 8
         };
 
-        const unsigned char *pYTable = yQuanTable518;
-        const unsigned char *pUVTable = uvQuanTable518;
+        const unsigned char *pYTable, *pUVTable;
         unsigned char val0, val1;
-        int i, rc, reg = R51x_COMP_LUT_BEGIN;
+        int i, size, rc, reg = R51x_COMP_LUT_BEGIN;
 
         PDEBUG(D_PROBE, "Uploading quantization tables");
 
-        for (i = 0; i < 16; i++) {
+        if (sd->bridge == BRIDGE_OV511 || sd->bridge == BRIDGE_OV511PLUS) {
+                pYTable = yQuanTable511;
+                pUVTable = uvQuanTable511;
+                size  = 32;
+        } else {
+                pYTable = yQuanTable518;
+                pUVTable = uvQuanTable518;
+                size  = 16;
+        }
+
+        for (i = 0; i < size; i++) {
                 val0 = *pYTable++;
                 val1 = *pYTable++;
                 val0 &= 0x0f;
@@ -1445,7 +1765,7 @@ static int ov518_upload_quan_tables(struct sd *sd)
                 val0 &= 0x0f;
                 val1 &= 0x0f;
                 val0 |= val1 << 4;
-                rc = reg_w(sd, reg + 16, val0);
+                rc = reg_w(sd, reg + size, val0);
                 if (rc < 0)
                         return rc;
 
@@ -1455,6 +1775,87 @@ static int ov518_upload_quan_tables(struct sd *sd)
         return 0;
 }
 
+/* This initializes the OV511/OV511+ and the sensor */
+static int ov511_configure(struct gspca_dev *gspca_dev)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+        int rc;
+
+        /* For 511 and 511+ */
+        const struct ov_regvals init_511[] = {
+                { R51x_SYS_RESET,       0x7f },
+                { R51x_SYS_INIT,        0x01 },
+                { R51x_SYS_RESET,       0x7f },
+                { R51x_SYS_INIT,        0x01 },
+                { R51x_SYS_RESET,       0x3f },
+                { R51x_SYS_INIT,        0x01 },
+                { R51x_SYS_RESET,       0x3d },
+        };
+
+        const struct ov_regvals norm_511[] = {
+                { R511_DRAM_FLOW_CTL,   0x01 },
+                { R51x_SYS_SNAP,        0x00 },
+                { R51x_SYS_SNAP,        0x02 },
+                { R51x_SYS_SNAP,        0x00 },
+                { R511_FIFO_OPTS,       0x1f },
+                { R511_COMP_EN,         0x00 },
+                { R511_COMP_LUT_EN,     0x03 },
+        };
+
+        const struct ov_regvals norm_511_p[] = {
+                { R511_DRAM_FLOW_CTL,   0xff },
+                { R51x_SYS_SNAP,        0x00 },
+                { R51x_SYS_SNAP,        0x02 },
+                { R51x_SYS_SNAP,        0x00 },
+                { R511_FIFO_OPTS,       0xff },
+                { R511_COMP_EN,         0x00 },
+                { R511_COMP_LUT_EN,     0x03 },
+        };
+
+        const struct ov_regvals compress_511[] = {
+                { 0x70, 0x1f },
+                { 0x71, 0x05 },
+                { 0x72, 0x06 },
+                { 0x73, 0x06 },
+                { 0x74, 0x14 },
+                { 0x75, 0x03 },
+                { 0x76, 0x04 },
+                { 0x77, 0x04 },
+        };
+
+        PDEBUG(D_PROBE, "Device custom id %x", reg_r(sd, R51x_SYS_CUST_ID));
+
+        rc = write_regvals(sd, init_511, ARRAY_SIZE(init_511));
+        if (rc < 0)
+                return rc;
+
+        switch (sd->bridge) {
+        case BRIDGE_OV511:
+                rc = write_regvals(sd, norm_511, ARRAY_SIZE(norm_511));
+                if (rc < 0)
+                        return rc;
+                break;
+        case BRIDGE_OV511PLUS:
+                rc = write_regvals(sd, norm_511_p, ARRAY_SIZE(norm_511_p));
+                if (rc < 0)
+                        return rc;
+                break;
+        }
+
+        /* Init compression */
+        rc = write_regvals(sd, compress_511, ARRAY_SIZE(compress_511));
+        if (rc < 0)
+                return rc;
+
+        rc = ov51x_upload_quan_tables(sd);
+        if (rc < 0) {
+                PDEBUG(D_ERR, "Error uploading quantization tables");
+                return rc;
+        }
+
+        return 0;
+}
+
 /* This initializes the OV518/OV518+ and the sensor */
 static int ov518_configure(struct gspca_dev *gspca_dev)
 {
@@ -1462,7 +1863,7 @@ static int ov518_configure(struct gspca_dev *gspca_dev)
         int rc;
 
         /* For 518 and 518+ */
-        static struct ov_regvals init_518[] = {
+        const struct ov_regvals init_518[] = {
                 { R51x_SYS_RESET,       0x40 },
                 { R51x_SYS_INIT,        0xe1 },
                 { R51x_SYS_RESET,       0x3e },
@@ -1473,7 +1874,7 @@ static int ov518_configure(struct gspca_dev *gspca_dev)
                 { 0x5d,                 0x03 },
         };
 
-        static struct ov_regvals norm_518[] = {
+        const struct ov_regvals norm_518[] = {
                 { R51x_SYS_SNAP,        0x02 }, /* Reset */
                 { R51x_SYS_SNAP,        0x01 }, /* Enable */
                 { 0x31,                 0x0f },
@@ -1486,7 +1887,7 @@ static int ov518_configure(struct gspca_dev *gspca_dev)
                 { 0x2f,                 0x80 },
         };
 
-        static struct ov_regvals norm_518_p[] = {
+        const struct ov_regvals norm_518_p[] = {
                 { R51x_SYS_SNAP,        0x02 }, /* Reset */
                 { R51x_SYS_SNAP,        0x01 }, /* Enable */
                 { 0x31,                 0x0f },
@@ -1531,7 +1932,7 @@ static int ov518_configure(struct gspca_dev *gspca_dev)
                 break;
         }
 
-        rc = ov518_upload_quan_tables(sd);
+        rc = ov51x_upload_quan_tables(sd);
         if (rc < 0) {
                 PDEBUG(D_ERR, "Error uploading quantization tables");
                 return rc;
@@ -1573,9 +1974,14 @@ static int sd_config(struct gspca_dev *gspca_dev,
         struct cam *cam;
         int ret = 0;
 
-        sd->bridge = id->driver_info;
+        sd->bridge = id->driver_info & BRIDGE_MASK;
+        sd->invert_led = id->driver_info & BRIDGE_INVERT_LED;
 
         switch (sd->bridge) {
+        case BRIDGE_OV511:
+        case BRIDGE_OV511PLUS:
+                ret = ov511_configure(gspca_dev);
+                break;
         case BRIDGE_OV518:
         case BRIDGE_OV518PLUS:
                 ret = ov518_configure(gspca_dev);
@@ -1634,6 +2040,16 @@ static int sd_config(struct gspca_dev *gspca_dev,
 
         cam = &gspca_dev->cam;
         switch (sd->bridge) {
+        case BRIDGE_OV511:
+        case BRIDGE_OV511PLUS:
+                if (!sd->sif) {
+                        cam->cam_mode = ov511_vga_mode;
+                        cam->nmodes = ARRAY_SIZE(ov511_vga_mode);
+                } else {
+                        cam->cam_mode = ov511_sif_mode;
+                        cam->nmodes = ARRAY_SIZE(ov511_sif_mode);
+                }
+                break;
         case BRIDGE_OV518:
         case BRIDGE_OV518PLUS:
                 if (!sd->sif) {
@@ -1655,13 +2071,28 @@ static int sd_config(struct gspca_dev *gspca_dev,
                 break;
         }
         sd->brightness = BRIGHTNESS_DEF;
-        sd->contrast = CONTRAST_DEF;
+        if (sd->sensor == SEN_OV6630 || sd->sensor == SEN_OV66308AF)
+                sd->contrast = 200; /* The default is too low for the ov6630 */
+        else
+                sd->contrast = CONTRAST_DEF;
         sd->colors = COLOR_DEF;
         sd->hflip = HFLIP_DEF;
         sd->vflip = VFLIP_DEF;
-        if (sd->sensor != SEN_OV7670)
-                gspca_dev->ctrl_dis = (1 << HFLIP_IDX)
-                                        | (1 << VFLIP_IDX);
+        sd->autobrightness = AUTOBRIGHT_DEF;
+        if (sd->sensor == SEN_OV7670) {
+                sd->freq = OV7670_FREQ_DEF;
+                gspca_dev->ctrl_dis = 1 << FREQ_IDX;
+        } else {
+                sd->freq = FREQ_DEF;
+                gspca_dev->ctrl_dis = (1 << HFLIP_IDX) | (1 << VFLIP_IDX) |
+                                      (1 << OV7670_FREQ_IDX);
+        }
+        if (sd->sensor == SEN_OV7640 || sd->sensor == SEN_OV7670)
+                gspca_dev->ctrl_dis |= 1 << AUTOBRIGHT_IDX;
+        /* OV8610 Frequency filter control should work but needs testing */
+        if (sd->sensor == SEN_OV8610)
+                gspca_dev->ctrl_dis |= 1 << FREQ_IDX;
+
         return 0;
 error:
         PDEBUG(D_ERR, "OV519 Config failed");
@@ -1680,6 +2111,7 @@ static int sd_init(struct gspca_dev *gspca_dev)
                         return -EIO;
                 break;
         case SEN_OV6630:
+        case SEN_OV66308AF:
                 if (write_i2c_regvals(sd, norm_6x30, ARRAY_SIZE(norm_6x30)))
                         return -EIO;
                 break;
@@ -1688,6 +2120,8 @@ static int sd_init(struct gspca_dev *gspca_dev)
 /*      case SEN_OV76BE: */
                 if (write_i2c_regvals(sd, norm_7610, ARRAY_SIZE(norm_7610)))
                         return -EIO;
+                if (i2c_w_mask(sd, 0x0e, 0x00, 0x40))
+                        return -EIO;
                 break;
         case SEN_OV7620:
                 if (write_i2c_regvals(sd, norm_7620, ARRAY_SIZE(norm_7620)))
@@ -1709,6 +2143,126 @@ static int sd_init(struct gspca_dev *gspca_dev)
         return 0;
 }
 
+/* Set up the OV511/OV511+ with the given image parameters.
+ *
+ * Do not put any sensor-specific code in here (including I2C I/O functions)
+ */
+static int ov511_mode_init_regs(struct sd *sd)
+{
+        int hsegs, vsegs, packet_size, fps, needed;
+        int interlaced = 0;
+        struct usb_host_interface *alt;
+        struct usb_interface *intf;
+
+        intf = usb_ifnum_to_if(sd->gspca_dev.dev, sd->gspca_dev.iface);
+        alt = usb_altnum_to_altsetting(intf, sd->gspca_dev.alt);
+        if (!alt) {
+                PDEBUG(D_ERR, "Couldn't get altsetting");
+                return -EIO;
+        }
+
+        packet_size = le16_to_cpu(alt->endpoint[0].desc.wMaxPacketSize);
+        reg_w(sd, R51x_FIFO_PSIZE, packet_size >> 5);
+
+        reg_w(sd, R511_CAM_UV_EN, 0x01);
+        reg_w(sd, R511_SNAP_UV_EN, 0x01);
+        reg_w(sd, R511_SNAP_OPTS, 0x03);
+
+        /* Here I'm assuming that snapshot size == image size.
+         * I hope that's always true. --claudio
+         */
+        hsegs = (sd->gspca_dev.width >> 3) - 1;
+        vsegs = (sd->gspca_dev.height >> 3) - 1;
+
+        reg_w(sd, R511_CAM_PXCNT, hsegs);
+        reg_w(sd, R511_CAM_LNCNT, vsegs);
+        reg_w(sd, R511_CAM_PXDIV, 0x00);
+        reg_w(sd, R511_CAM_LNDIV, 0x00);
+
+        /* YUV420, low pass filter on */
+        reg_w(sd, R511_CAM_OPTS, 0x03);
+
+        /* Snapshot additions */
+        reg_w(sd, R511_SNAP_PXCNT, hsegs);
+        reg_w(sd, R511_SNAP_LNCNT, vsegs);
+        reg_w(sd, R511_SNAP_PXDIV, 0x00);
+        reg_w(sd, R511_SNAP_LNDIV, 0x00);
+
+        /******** Set the framerate ********/
+        if (frame_rate > 0)
+                sd->frame_rate = frame_rate;
+
+        switch (sd->sensor) {
+        case SEN_OV6620:
+                /* No framerate control, doesn't like higher rates yet */
+                sd->clockdiv = 3;
+                break;
+
+        /* Note once the FIXME's in mode_init_ov_sensor_regs() are fixed
+           for more sensors we need to do this for them too */
+        case SEN_OV7620:
+        case SEN_OV7640:
+        case SEN_OV76BE:
+                if (sd->gspca_dev.width == 320)
+                        interlaced = 1;
+                /* Fall through */
+        case SEN_OV6630:
+        case SEN_OV7610:
+        case SEN_OV7670:
+                switch (sd->frame_rate) {
+                case 30:
+                case 25:
+                        /* Not enough bandwidth to do 640x480 @ 30 fps */
+                        if (sd->gspca_dev.width != 640) {
+                                sd->clockdiv = 0;
+                                break;
+                        }
+                        /* Fall through for 640x480 case */
+                default:
+/*              case 20: */
+/*              case 15: */
+                        sd->clockdiv = 1;
+                        break;
+                case 10:
+                        sd->clockdiv = 2;
+                        break;
+                case 5:
+                        sd->clockdiv = 5;
+                        break;
+                }
+                if (interlaced) {
+                        sd->clockdiv = (sd->clockdiv + 1) * 2 - 1;
+                        /* Higher then 10 does not work */
+                        if (sd->clockdiv > 10)
+                                sd->clockdiv = 10;
+                }
+                break;
+
+        case SEN_OV8610:
+                /* No framerate control ?? */
+                sd->clockdiv = 0;
+                break;
+        }
+
+        /* Check if we have enough bandwidth to disable compression */
+        fps = (interlaced ? 60 : 30) / (sd->clockdiv + 1) + 1;
+        needed = fps * sd->gspca_dev.width * sd->gspca_dev.height * 3 / 2;
+        /* 1400 is a conservative estimate of the max nr of isoc packets/sec */
+        if (needed > 1400 * packet_size) {
+                /* Enable Y and UV quantization and compression */
+                reg_w(sd, R511_COMP_EN, 0x07);
+                reg_w(sd, R511_COMP_LUT_EN, 0x03);
+        } else {
+                reg_w(sd, R511_COMP_EN, 0x06);
+                reg_w(sd, R511_COMP_LUT_EN, 0x00);
+        }
+
+        reg_w(sd, R51x_SYS_RESET, OV511_RESET_OMNICE);
+        reg_w(sd, R51x_SYS_RESET, 0);
+
+        return 0;
+}
+
 /* Sets up the OV518/OV518+ with the given image parameters
  *
  * OV518 needs a completely different approach, until we can figure out what
@@ -1718,7 +2272,19 @@ static int sd_init(struct gspca_dev *gspca_dev)
  */
 static int ov518_mode_init_regs(struct sd *sd)
 {
-        int hsegs, vsegs;
+        int hsegs, vsegs, packet_size;
+        struct usb_host_interface *alt;
+        struct usb_interface *intf;
+
+        intf = usb_ifnum_to_if(sd->gspca_dev.dev, sd->gspca_dev.iface);
+        alt = usb_altnum_to_altsetting(intf, sd->gspca_dev.alt);
+        if (!alt) {
+                PDEBUG(D_ERR, "Couldn't get altsetting");
+                return -EIO;
+        }
+
+        packet_size = le16_to_cpu(alt->endpoint[0].desc.wMaxPacketSize);
+        ov518_reg_w32(sd, R51x_FIFO_PSIZE, packet_size & ~7, 2);
 
         /******** Set the mode ********/
 
@@ -1755,20 +2321,30 @@ static int ov518_mode_init_regs(struct sd *sd)
         /* Windows driver does this here; who knows why */
         reg_w(sd, 0x2f, 0x80);
 
-        /******** Set the framerate (to 30 FPS) ********/
-        if (sd->bridge == BRIDGE_OV518PLUS)
-                sd->clockdiv = 1;
-        else
-                sd->clockdiv = 0;
+        /******** Set the framerate  ********/
+        sd->clockdiv = 1;
 
         /* Mode independent, but framerate dependent, regs */
-        reg_w(sd, 0x51, 0x04);  /* Clock divider; lower==faster */
+        /* 0x51: Clock divider; Only works on some cams which use 2 crystals */
+        reg_w(sd, 0x51, 0x04);
         reg_w(sd, 0x22, 0x18);
         reg_w(sd, 0x23, 0xff);
 
-        if (sd->bridge == BRIDGE_OV518PLUS)
-                reg_w(sd, 0x21, 0x19);
-        else
+        if (sd->bridge == BRIDGE_OV518PLUS) {
+                switch (sd->sensor) {
+                case SEN_OV7620:
+                        if (sd->gspca_dev.width == 320) {
+                                reg_w(sd, 0x20, 0x00);
+                                reg_w(sd, 0x21, 0x19);
+                        } else {
+                                reg_w(sd, 0x20, 0x60);
+                                reg_w(sd, 0x21, 0x1f);
+                        }
+                        break;
+                default:
+                        reg_w(sd, 0x21, 0x19);
+                }
+        } else
                 reg_w(sd, 0x71, 0x17);  /* Compression-related? */
 
         /* FIXME: Sensor-specific */
@@ -1879,7 +2455,11 @@ static int ov519_mode_init_regs(struct sd *sd)
 
         reg_w(sd, OV519_R10_H_SIZE,     sd->gspca_dev.width >> 4);
         reg_w(sd, OV519_R11_V_SIZE,     sd->gspca_dev.height >> 3);
-        reg_w(sd, OV519_R12_X_OFFSETL,  0x00);
+        if (sd->sensor == SEN_OV7670 &&
+            sd->gspca_dev.cam.cam_mode[sd->gspca_dev.curr_mode].priv)
+                reg_w(sd, OV519_R12_X_OFFSETL, 0x04);
+        else
+                reg_w(sd, OV519_R12_X_OFFSETL, 0x00);
         reg_w(sd, OV519_R13_X_OFFSETH,  0x00);
         reg_w(sd, OV519_R14_Y_OFFSETL,  0x00);
         reg_w(sd, OV519_R15_Y_OFFSETH,  0x00);
@@ -1971,7 +2551,7 @@ static int mode_init_ov_sensor_regs(struct sd *sd)
         int qvga;
 
         gspca_dev = &sd->gspca_dev;
-        qvga = gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv;
+        qvga = gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv & 1;
 
         /******** Mode (VGA/QVGA) and sensor specific regs ********/
         switch (sd->sensor) {
@@ -1983,21 +2563,16 @@ static int mode_init_ov_sensor_regs(struct sd *sd)
                 i2c_w_mask(sd, 0x14, qvga ? 0x20 : 0x00, 0x20);
                 break;
         case SEN_OV7620:
-/*              i2c_w(sd, 0x2b, 0x00); */
+        case SEN_OV76BE:
                 i2c_w_mask(sd, 0x14, qvga ? 0x20 : 0x00, 0x20);
                 i2c_w_mask(sd, 0x28, qvga ? 0x00 : 0x20, 0x20);
                 i2c_w(sd, 0x24, qvga ? 0x20 : 0x3a);
                 i2c_w(sd, 0x25, qvga ? 0x30 : 0x60);
                 i2c_w_mask(sd, 0x2d, qvga ? 0x40 : 0x00, 0x40);
-                i2c_w_mask(sd, 0x67, qvga ? 0xf0 : 0x90, 0xf0);
+                i2c_w_mask(sd, 0x67, qvga ? 0xb0 : 0x90, 0xf0);
                 i2c_w_mask(sd, 0x74, qvga ? 0x20 : 0x00, 0x20);
                 break;
-        case SEN_OV76BE:
-/*              i2c_w(sd, 0x2b, 0x00); */
-                i2c_w_mask(sd, 0x14, qvga ? 0x20 : 0x00, 0x20);
-                break;
         case SEN_OV7640:
-/*              i2c_w(sd, 0x2b, 0x00); */
                 i2c_w_mask(sd, 0x14, qvga ? 0x20 : 0x00, 0x20);
                 i2c_w_mask(sd, 0x28, qvga ? 0x00 : 0x20, 0x20);
 /*              i2c_w(sd, 0x24, qvga ? 0x20 : 0x3a); */
@@ -2016,6 +2591,7 @@ static int mode_init_ov_sensor_regs(struct sd *sd)
                 break;
         case SEN_OV6620:
         case SEN_OV6630:
+        case SEN_OV66308AF:
                 i2c_w_mask(sd, 0x14, qvga ? 0x20 : 0x00, 0x20);
                 break;
         default:
@@ -2023,10 +2599,6 @@ static int mode_init_ov_sensor_regs(struct sd *sd)
         }
 
         /******** Palette-specific regs ********/
-        if (sd->sensor == SEN_OV7610 || sd->sensor == SEN_OV76BE) {
-                /* not valid on the OV6620/OV7620/6630? */
-                i2c_w_mask(sd, 0x0e, 0x00, 0x40);
-        }
 
         /* The OV518 needs special treatment. Although both the OV518
          * and the OV6630 support a 16-bit video bus, only the 8 bit Y
@@ -2036,25 +2608,12 @@ static int mode_init_ov_sensor_regs(struct sd *sd)
 
         /* OV7640 is 8-bit only */
 
-        if (sd->sensor != SEN_OV6630 && sd->sensor != SEN_OV7640)
+        if (sd->sensor != SEN_OV6630 && sd->sensor != SEN_OV66308AF &&
+                                        sd->sensor != SEN_OV7640)
                 i2c_w_mask(sd, 0x13, 0x00, 0x20);
 
         /******** Clock programming ********/
-        /* The OV6620 needs special handling. This prevents the
-         * severe banding that normally occurs */
-        if (sd->sensor == SEN_OV6620) {
-
-                /* Clock down */
-                i2c_w(sd, 0x2a, 0x04);
-                i2c_w(sd, 0x11, sd->clockdiv);
-                i2c_w(sd, 0x2a, 0x84);
-                /* This next setting is critical. It seems to improve
-                 * the gain or the contrast. The "reserved" bits seem
-                 * to have some effect in this case. */
-                i2c_w(sd, 0x2d, 0x85);
-        } else {
-                i2c_w(sd, 0x11, sd->clockdiv);
-        }
+        i2c_w(sd, 0x11, sd->clockdiv);
 
         /******** Special Features ********/
 /* no evidence this is possible with OV7670, either */
@@ -2098,13 +2657,14 @@ static void sethvflip(struct sd *sd)
 static int set_ov_sensor_window(struct sd *sd)
 {
         struct gspca_dev *gspca_dev;
-        int qvga;
+        int qvga, crop;
         int hwsbase, hwebase, vwsbase, vwebase, hwscale, vwscale;
         int ret, hstart, hstop, vstop, vstart;
         __u8 v;
 
         gspca_dev = &sd->gspca_dev;
-        qvga = gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv;
+        qvga = gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv & 1;
+        crop = gspca_dev->cam.cam_mode[(int) gspca_dev->curr_mode].priv & 2;
 
         /* The different sensor ICs handle setting up of window differently.
          * IF YOU SET IT WRONG, YOU WILL GET ALL ZERO ISOC DATA FROM OV51x!! */
@@ -2123,14 +2683,19 @@ static int set_ov_sensor_window(struct sd *sd)
                 break;
         case SEN_OV6620:
         case SEN_OV6630:
+        case SEN_OV66308AF:
                 hwsbase = 0x38;
                 hwebase = 0x3a;
                 vwsbase = 0x05;
                 vwebase = 0x06;
-                if (qvga) {
+                if (sd->sensor == SEN_OV66308AF && qvga)
                         /* HDG: this fixes U and V getting swapped */
-                        hwsbase--;
-                        vwsbase--;
+                        hwsbase++;
+                if (crop) {
+                        hwsbase += 8;
+                        hwebase += 8;
+                        vwsbase += 11;
+                        vwebase += 11;
                 }
                 break;
         case SEN_OV7620:
@@ -2155,6 +2720,7 @@ static int set_ov_sensor_window(struct sd *sd)
         switch (sd->sensor) {
         case SEN_OV6620:
         case SEN_OV6630:
+        case SEN_OV66308AF:
                 if (qvga) {             /* QCIF */
                         hwscale = 0;
                         vwscale = 0;
@@ -2207,7 +2773,7 @@ static int set_ov_sensor_window(struct sd *sd)
                 if (qvga) {             /* QVGA from ov7670.c by
                                          * Jonathan Corbet */
                         hstart = 164;
-                        hstop = 20;
+                        hstop = 28;
                         vstart = 14;
                         vstop = 494;
                 } else {                /* VGA */
@@ -2233,7 +2799,6 @@ static int set_ov_sensor_window(struct sd *sd)
                 msleep(10);     /* need to sleep between read and write to
                                  * same reg! */
                 i2c_w(sd, OV7670_REG_VREF, v);
-                sethvflip(sd);
         } else {
                 i2c_w(sd, 0x17, hwsbase);
                 i2c_w(sd, 0x18, hwebase + (sd->gspca_dev.width >> hwscale));
@@ -2250,6 +2815,10 @@ static int sd_start(struct gspca_dev *gspca_dev)
         int ret = 0;
 
         switch (sd->bridge) {
+        case BRIDGE_OV511:
+        case BRIDGE_OV511PLUS:
+                ret = ov511_mode_init_regs(sd);
+                break;
         case BRIDGE_OV518:
         case BRIDGE_OV518PLUS:
                 ret = ov518_mode_init_regs(sd);
@@ -2268,6 +2837,9 @@ static int sd_start(struct gspca_dev *gspca_dev)
         setcontrast(gspca_dev);
         setbrightness(gspca_dev);
         setcolors(gspca_dev);
+        sethvflip(sd);
+        setautobrightness(sd);
+        setfreq(sd);
 
         ret = ov51x_restart(sd);
         if (ret < 0)
@@ -2287,23 +2859,88 @@ static void sd_stopN(struct gspca_dev *gspca_dev)
         ov51x_led_control(sd, 0);
 }
 
-static void ov518_pkt_scan(struct gspca_dev *gspca_dev,
+static void ov511_pkt_scan(struct gspca_dev *gspca_dev,
                         struct gspca_frame *frame,      /* target */
-                        __u8 *data,                     /* isoc packet */
+                        __u8 *in,                       /* isoc packet */
                         int len)                        /* iso packet length */
 {
-        PDEBUG(D_STREAM, "ov518_pkt_scan: %d bytes", len);
+        struct sd *sd = (struct sd *) gspca_dev;
 
-        if (len & 7) {
-                len--;
-                PDEBUG(D_STREAM, "packet number: %d\n", (int)data[len]);
+        /* SOF/EOF packets have 1st to 8th bytes zeroed and the 9th
+         * byte non-zero. The EOF packet has image width/height in the
+         * 10th and 11th bytes. The 9th byte is given as follows:
+         *
+         * bit 7: EOF
+         *     6: compression enabled
+         *     5: 422/420/400 modes
+         *     4: 422/420/400 modes
+         *     3: 1
+         *     2: snapshot button on
+         *     1: snapshot frame
+         *     0: even/odd field
+         */
+        if (!(in[0] | in[1] | in[2] | in[3] | in[4] | in[5] | in[6] | in[7]) &&
+            (in[8] & 0x08)) {
+                if (in[8] & 0x80) {
+                        /* Frame end */
+                        if ((in[9] + 1) * 8 != gspca_dev->width ||
+                            (in[10] + 1) * 8 != gspca_dev->height) {
+                                PDEBUG(D_ERR, "Invalid frame size, got: %dx%d,"
+                                        " requested: %dx%d\n",
+                                        (in[9] + 1) * 8, (in[10] + 1) * 8,
+                                        gspca_dev->width, gspca_dev->height);
+                                gspca_dev->last_packet_type = DISCARD_PACKET;
+                                return;
+                        }
+                        /* Add 11 byte footer to frame, might be usefull */
+                        gspca_frame_add(gspca_dev, LAST_PACKET, frame, in, 11);
+                        return;
+                } else {
+                        /* Frame start */
+                        gspca_frame_add(gspca_dev, FIRST_PACKET, frame, in, 0);
+                        sd->packet_nr = 0;
+                }
         }
 
+        /* Ignore the packet number */
+        len--;
+
+        /* intermediate packet */
+        gspca_frame_add(gspca_dev, INTER_PACKET, frame, in, len);
+}
+
+static void ov518_pkt_scan(struct gspca_dev *gspca_dev,
+                        struct gspca_frame *frame,      /* target */
+                        __u8 *data,                     /* isoc packet */
+                        int len)                        /* iso packet length */
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+
         /* A false positive here is likely, until OVT gives me
          * the definitive SOF/EOF format */
         if ((!(data[0] | data[1] | data[2] | data[3] | data[5])) && data[6]) {
                 gspca_frame_add(gspca_dev, LAST_PACKET, frame, data, 0);
                 gspca_frame_add(gspca_dev, FIRST_PACKET, frame, data, 0);
+                sd->packet_nr = 0;
+        }
+
+        if (gspca_dev->last_packet_type == DISCARD_PACKET)
+                return;
+
+        /* Does this device use packet numbers ? */
+        if (len & 7) {
+                len--;
+                if (sd->packet_nr == data[len])
+                        sd->packet_nr++;
+                /* The last few packets of the frame (which are all 0's
+                   except that they may contain part of the footer), are
+                   numbered 0 */
+                else if (sd->packet_nr == 0 || data[len]) {
+                        PDEBUG(D_ERR, "Invalid packet nr: %d (expect: %d)",
+                                (int)data[len], (int)sd->packet_nr);
+                        gspca_dev->last_packet_type = DISCARD_PACKET;
+                        return;
+                }
         }
 
         /* intermediate packet */
@@ -2364,6 +3001,7 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
         switch (sd->bridge) {
         case BRIDGE_OV511:
         case BRIDGE_OV511PLUS:
+                ov511_pkt_scan(gspca_dev, frame, data, len);
                 break;
         case BRIDGE_OV518:
         case BRIDGE_OV518PLUS:
@@ -2389,13 +3027,13 @@ static void setbrightness(struct gspca_dev *gspca_dev)
         case SEN_OV76BE:
         case SEN_OV6620:
         case SEN_OV6630:
+        case SEN_OV66308AF:
         case SEN_OV7640:
                 i2c_w(sd, OV7610_REG_BRT, val);
                 break;
         case SEN_OV7620:
                 /* 7620 doesn't like manual changes when in auto mode */
-/*fixme
- *              if (!sd->auto_brt) */
+                if (!sd->autobrightness)
                         i2c_w(sd, OV7610_REG_BRT, val);
                 break;
         case SEN_OV7670:
@@ -2418,6 +3056,7 @@ static void setcontrast(struct gspca_dev *gspca_dev)
                 i2c_w(sd, OV7610_REG_CNT, val);
                 break;
         case SEN_OV6630:
+        case SEN_OV66308AF:
                 i2c_w_mask(sd, OV7610_REG_CNT, val >> 4, 0x0f);
                 break;
         case SEN_OV8610: {
@@ -2462,6 +3101,7 @@ static void setcolors(struct gspca_dev *gspca_dev)
         case SEN_OV76BE:
         case SEN_OV6620:
         case SEN_OV6630:
+        case SEN_OV66308AF:
                 i2c_w(sd, OV7610_REG_SAT, val);
                 break;
         case SEN_OV7620:
@@ -2482,6 +3122,72 @@ static void setcolors(struct gspca_dev *gspca_dev)
         }
 }
 
+static void setautobrightness(struct sd *sd)
+{
+        if (sd->sensor == SEN_OV7640 || sd->sensor == SEN_OV7670)
+                return;
+
+        i2c_w_mask(sd, 0x2d, sd->autobrightness ? 0x10 : 0x00, 0x10);
+}
+
+static void setfreq(struct sd *sd)
+{
+        if (sd->sensor == SEN_OV7670) {
+                switch (sd->freq) {
+                case 0: /* Banding filter disabled */
+                        i2c_w_mask(sd, OV7670_REG_COM8, 0, OV7670_COM8_BFILT);
+                        break;
+                case 1: /* 50 hz */
+                        i2c_w_mask(sd, OV7670_REG_COM8, OV7670_COM8_BFILT,
+                                   OV7670_COM8_BFILT);
+                        i2c_w_mask(sd, OV7670_REG_COM11, 0x08, 0x18);
+                        break;
+                case 2: /* 60 hz */
+                        i2c_w_mask(sd, OV7670_REG_COM8, OV7670_COM8_BFILT,
+                                   OV7670_COM8_BFILT);
+                        i2c_w_mask(sd, OV7670_REG_COM11, 0x00, 0x18);
+                        break;
+                case 3: /* Auto hz */
+                        i2c_w_mask(sd, OV7670_REG_COM8, OV7670_COM8_BFILT,
+                                   OV7670_COM8_BFILT);
+                        i2c_w_mask(sd, OV7670_REG_COM11, OV7670_COM11_HZAUTO,
+                                   0x18);
+                        break;
+                }
+        } else {
+                switch (sd->freq) {
+                case 0: /* Banding filter disabled */
+                        i2c_w_mask(sd, 0x2d, 0x00, 0x04);
+                        i2c_w_mask(sd, 0x2a, 0x00, 0x80);
+                        break;
+                case 1: /* 50 hz (filter on and framerate adj) */
+                        i2c_w_mask(sd, 0x2d, 0x04, 0x04);
+                        i2c_w_mask(sd, 0x2a, 0x80, 0x80);
+                        /* 20 fps -> 16.667 fps */
+                        if (sd->sensor == SEN_OV6620 ||
+                            sd->sensor == SEN_OV6630 ||
+                            sd->sensor == SEN_OV66308AF)
+                                i2c_w(sd, 0x2b, 0x5e);
+                        else
+                                i2c_w(sd, 0x2b, 0xac);
+                        break;
+                case 2: /* 60 hz (filter on, ...) */
+                        i2c_w_mask(sd, 0x2d, 0x04, 0x04);
+                        if (sd->sensor == SEN_OV6620 ||
+                            sd->sensor == SEN_OV6630 ||
+                            sd->sensor == SEN_OV66308AF) {
+                                /* 20 fps -> 15 fps */
+                                i2c_w_mask(sd, 0x2a, 0x80, 0x80);
+                                i2c_w(sd, 0x2b, 0xa8);
+                        } else {
+                                /* no framerate adj. */
+                                i2c_w_mask(sd, 0x2a, 0x00, 0x80);
+                        }
+                        break;
+                }
+        }
+}
+
 static int sd_setbrightness(struct gspca_dev *gspca_dev, __s32 val)
 {
         struct sd *sd = (struct sd *) gspca_dev;
@@ -2572,6 +3278,71 @@ static int sd_getvflip(struct gspca_dev *gspca_dev, __s32 *val)
         return 0;
 }
 
+static int sd_setautobrightness(struct gspca_dev *gspca_dev, __s32 val)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+
+        sd->autobrightness = val;
+        if (gspca_dev->streaming)
+                setautobrightness(sd);
+        return 0;
+}
+
+static int sd_getautobrightness(struct gspca_dev *gspca_dev, __s32 *val)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+
+        *val = sd->autobrightness;
+        return 0;
+}
+
+static int sd_setfreq(struct gspca_dev *gspca_dev, __s32 val)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+
+        sd->freq = val;
+        if (gspca_dev->streaming)
+                setfreq(sd);
+        return 0;
+}
+
+static int sd_getfreq(struct gspca_dev *gspca_dev, __s32 *val)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+
+        *val = sd->freq;
+        return 0;
+}
+
+static int sd_querymenu(struct gspca_dev *gspca_dev,
+                        struct v4l2_querymenu *menu)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+
+        switch (menu->id) {
+        case V4L2_CID_POWER_LINE_FREQUENCY:
+                switch (menu->index) {
+                case 0:         /* V4L2_CID_POWER_LINE_FREQUENCY_DISABLED */
+                        strcpy((char *) menu->name, "NoFliker");
+                        return 0;
+                case 1:         /* V4L2_CID_POWER_LINE_FREQUENCY_50HZ */
+                        strcpy((char *) menu->name, "50 Hz");
+                        return 0;
+                case 2:         /* V4L2_CID_POWER_LINE_FREQUENCY_60HZ */
+                        strcpy((char *) menu->name, "60 Hz");
+                        return 0;
+                case 3:
+                        if (sd->sensor != SEN_OV7670)
+                                return -EINVAL;
+
+                        strcpy((char *) menu->name, "Automatic");
+                        return 0;
+                }
+                break;
+        }
+        return -EINVAL;
+}
+
 /* sub-driver description */
 static const struct sd_desc sd_desc = {
         .name = MODULE_NAME,
@@ -2582,6 +3353,7 @@ static const struct sd_desc sd_desc = {
         .start = sd_start,
         .stopN = sd_stopN,
         .pkt_scan = sd_pkt_scan,
+        .querymenu = sd_querymenu,
 };
 
 /* -- module initialisation -- */
@@ -2590,17 +3362,22 @@ static const __devinitdata struct usb_device_id device_table[] = {
         {USB_DEVICE(0x041e, 0x405f), .driver_info = BRIDGE_OV519 },
         {USB_DEVICE(0x041e, 0x4060), .driver_info = BRIDGE_OV519 },
         {USB_DEVICE(0x041e, 0x4061), .driver_info = BRIDGE_OV519 },
-        {USB_DEVICE(0x041e, 0x4064), .driver_info = BRIDGE_OV519 },
-        {USB_DEVICE(0x041e, 0x4068), .driver_info = BRIDGE_OV519 },
+        {USB_DEVICE(0x041e, 0x4064),
+         .driver_info = BRIDGE_OV519 | BRIDGE_INVERT_LED },
+        {USB_DEVICE(0x041e, 0x4068),
+         .driver_info = BRIDGE_OV519 | BRIDGE_INVERT_LED },
         {USB_DEVICE(0x045e, 0x028c), .driver_info = BRIDGE_OV519 },
         {USB_DEVICE(0x054c, 0x0154), .driver_info = BRIDGE_OV519 },
         {USB_DEVICE(0x054c, 0x0155), .driver_info = BRIDGE_OV519 },
+        {USB_DEVICE(0x05a9, 0x0511), .driver_info = BRIDGE_OV511 },
         {USB_DEVICE(0x05a9, 0x0518), .driver_info = BRIDGE_OV518 },
         {USB_DEVICE(0x05a9, 0x0519), .driver_info = BRIDGE_OV519 },
         {USB_DEVICE(0x05a9, 0x0530), .driver_info = BRIDGE_OV519 },
         {USB_DEVICE(0x05a9, 0x4519), .driver_info = BRIDGE_OV519 },
         {USB_DEVICE(0x05a9, 0x8519), .driver_info = BRIDGE_OV519 },
+        {USB_DEVICE(0x05a9, 0xa511), .driver_info = BRIDGE_OV511PLUS },
         {USB_DEVICE(0x05a9, 0xa518), .driver_info = BRIDGE_OV518PLUS },
+        {USB_DEVICE(0x0813, 0x0002), .driver_info = BRIDGE_OV511PLUS },
         {}
 };
 
diff --git a/drivers/media/video/gspca/sonixj.c b/drivers/media/video/gspca/sonixj.c
index dc6a6f11354a..0d02f41fa7d0 100644
--- a/drivers/media/video/gspca/sonixj.c
+++ b/drivers/media/video/gspca/sonixj.c
@@ -46,6 +46,7 @@ struct sd {
         u8 gamma;
         u8 vflip;                       /* ov7630/ov7648 only */
         u8 infrared;                    /* mt9v111 only */
+        u8 freq;                        /* ov76xx only */
         u8 quality;                     /* image quality */
 #define QUALITY_MIN 60
 #define QUALITY_MAX 95
@@ -96,8 +97,11 @@ static int sd_setvflip(struct gspca_dev *gspca_dev, __s32 val);
 static int sd_getvflip(struct gspca_dev *gspca_dev, __s32 *val);
 static int sd_setinfrared(struct gspca_dev *gspca_dev, __s32 val);
 static int sd_getinfrared(struct gspca_dev *gspca_dev, __s32 *val);
+static int sd_setfreq(struct gspca_dev *gspca_dev, __s32 val);
+static int sd_getfreq(struct gspca_dev *gspca_dev, __s32 *val);
 
 static struct ctrl sd_ctrls[] = {
+#define BRIGHTNESS_IDX 0
         {
             {
                 .id      = V4L2_CID_BRIGHTNESS,
@@ -113,6 +117,7 @@ static struct ctrl sd_ctrls[] = {
             .set = sd_setbrightness,
             .get = sd_getbrightness,
         },
+#define CONTRAST_IDX 1
         {
             {
                 .id      = V4L2_CID_CONTRAST,
@@ -128,20 +133,22 @@ static struct ctrl sd_ctrls[] = {
             .set = sd_setcontrast,
             .get = sd_getcontrast,
         },
+#define COLOR_IDX 2
         {
             {
                 .id      = V4L2_CID_SATURATION,
                 .type    = V4L2_CTRL_TYPE_INTEGER,
-                .name    = "Color",
+                .name    = "Saturation",
                 .minimum = 0,
                 .maximum = 40,
                 .step    = 1,
-#define COLOR_DEF 32
+#define COLOR_DEF 25
                 .default_value = COLOR_DEF,
             },
             .set = sd_setcolors,
             .get = sd_getcolors,
         },
+#define BLUE_BALANCE_IDX 3
         {
             {
                 .id      = V4L2_CID_BLUE_BALANCE,
@@ -156,6 +163,7 @@ static struct ctrl sd_ctrls[] = {
             .set = sd_setblue_balance,
             .get = sd_getblue_balance,
         },
+#define RED_BALANCE_IDX 4
         {
             {
                 .id      = V4L2_CID_RED_BALANCE,
@@ -170,6 +178,7 @@ static struct ctrl sd_ctrls[] = {
             .set = sd_setred_balance,
             .get = sd_getred_balance,
         },
+#define GAMMA_IDX 5
         {
             {
                 .id      = V4L2_CID_GAMMA,
@@ -184,7 +193,7 @@ static struct ctrl sd_ctrls[] = {
             .set = sd_setgamma,
             .get = sd_getgamma,
         },
-#define AUTOGAIN_IDX 5
+#define AUTOGAIN_IDX 6
         {
             {
                 .id      = V4L2_CID_AUTOGAIN,
@@ -200,7 +209,7 @@ static struct ctrl sd_ctrls[] = {
             .get = sd_getautogain,
         },
 /* ov7630/ov7648 only */
-#define VFLIP_IDX 6
+#define VFLIP_IDX 7
         {
             {
                 .id      = V4L2_CID_VFLIP,
@@ -209,14 +218,14 @@ static struct ctrl sd_ctrls[] = {
                 .minimum = 0,
                 .maximum = 1,
                 .step    = 1,
-#define VFLIP_DEF 0                     /* vflip def = 1 for ov7630 */
+#define VFLIP_DEF 0
                 .default_value = VFLIP_DEF,
             },
             .set = sd_setvflip,
             .get = sd_getvflip,
         },
 /* mt9v111 only */
-#define INFRARED_IDX 7
+#define INFRARED_IDX 8
         {
             {
                 .id      = V4L2_CID_INFRARED,
@@ -231,28 +240,44 @@ static struct ctrl sd_ctrls[] = {
             .set = sd_setinfrared,
             .get = sd_getinfrared,
         },
+/* ov7630/ov7648/ov7660 only */
+#define FREQ_IDX 9
+        {
+            {
+                .id      = V4L2_CID_POWER_LINE_FREQUENCY,
+                .type    = V4L2_CTRL_TYPE_MENU,
+                .name    = "Light frequency filter",
+                .minimum = 0,
+                .maximum = 2,   /* 0: 0, 1: 50Hz, 2:60Hz */
+                .step    = 1,
+#define FREQ_DEF 2
+                .default_value = FREQ_DEF,
+            },
+            .set = sd_setfreq,
+            .get = sd_getfreq,
+        },
 };
 
 /* table of the disabled controls */
 static __u32 ctrl_dis[] = {
-        (1 << INFRARED_IDX) | (1 << VFLIP_IDX),
+        (1 << INFRARED_IDX) | (1 << VFLIP_IDX) | (1 << FREQ_IDX),
                                                 /* SENSOR_HV7131R 0 */
-        (1 << INFRARED_IDX) | (1 << VFLIP_IDX),
+        (1 << INFRARED_IDX) | (1 << VFLIP_IDX) | (1 << FREQ_IDX),
                                                 /* SENSOR_MI0360 1 */
-        (1 << INFRARED_IDX) | (1 << VFLIP_IDX),
+        (1 << INFRARED_IDX) | (1 << VFLIP_IDX) | (1 << FREQ_IDX),
                                                 /* SENSOR_MO4000 2 */
-        (1 << VFLIP_IDX),
+        (1 << VFLIP_IDX) | (1 << FREQ_IDX),
                                                 /* SENSOR_MT9V111 3 */
-        (1 << INFRARED_IDX) | (1 << VFLIP_IDX),
+        (1 << INFRARED_IDX) | (1 << VFLIP_IDX) | (1 << FREQ_IDX),
                                                 /* SENSOR_OM6802 4 */
-        (1 << AUTOGAIN_IDX) | (1 << INFRARED_IDX),
+        (1 << INFRARED_IDX),
                                                 /* SENSOR_OV7630 5 */
         (1 << INFRARED_IDX),
                                                 /* SENSOR_OV7648 6 */
         (1 << AUTOGAIN_IDX) | (1 << INFRARED_IDX) | (1 << VFLIP_IDX),
                                                 /* SENSOR_OV7660 7 */
-        (1 << AUTOGAIN_IDX) | (1 << INFRARED_IDX) | (1 << VFLIP_IDX),
-                                                /* SENSOR_SP80708 8 */
+        (1 << AUTOGAIN_IDX) | (1 << INFRARED_IDX) | (1 << VFLIP_IDX) |
+                              (1 << FREQ_IDX),  /* SENSOR_SP80708 8 */
 };
 
 static const struct v4l2_pix_format vga_mode[] = {
@@ -268,7 +293,8 @@ static const struct v4l2_pix_format vga_mode[] = {
                 .priv = 1},
         {640, 480, V4L2_PIX_FMT_JPEG, V4L2_FIELD_NONE,
                 .bytesperline = 640,
-                .sizeimage = 640 * 480 * 3 / 8 + 590,
+                /* Note 3 / 8 is not large enough, not even 5 / 8 is ?! */
+                .sizeimage = 640 * 480 * 3 / 4 + 590,
                 .colorspace = V4L2_COLORSPACE_JPEG,
                 .priv = 0},
 };
@@ -604,7 +630,9 @@ static const u8 ov7630_sensor_init[][8] = {
 /* win: i2c_r from 00 to 80 */
         {0xd1, 0x21, 0x03, 0x80, 0x10, 0x20, 0x80, 0x10},
         {0xb1, 0x21, 0x0c, 0x20, 0x20, 0x00, 0x00, 0x10},
-        {0xd1, 0x21, 0x11, 0x00, 0x48, 0xc0, 0x00, 0x10},
+/* HDG: 0x11 was 0x00 change to 0x01 for better exposure (15 fps instead of 30)
+        0x13 was 0xc0 change to 0xc3 for auto gain and exposure */
+        {0xd1, 0x21, 0x11, 0x01, 0x48, 0xc3, 0x00, 0x10},
         {0xb1, 0x21, 0x15, 0x80, 0x03, 0x00, 0x00, 0x10},
         {0xd1, 0x21, 0x17, 0x1b, 0xbd, 0x05, 0xf6, 0x10},
         {0xa1, 0x21, 0x1b, 0x04, 0x00, 0x00, 0x00, 0x10},
@@ -638,9 +666,8 @@ static const u8 ov7630_sensor_init[][8] = {
         {0xa1, 0x21, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10},
         {0xb1, 0x21, 0x01, 0x80, 0x80, 0x00, 0x00, 0x10},
 /* */
-        {0xa1, 0x21, 0x11, 0x00, 0x00, 0x00, 0x00, 0x10},
-        {0xa1, 0x21, 0x2a, 0x88, 0x00, 0x00, 0x00, 0x10},
-        {0xa1, 0x21, 0x2b, 0x34, 0x00, 0x00, 0x00, 0x10},
+/*      {0xa1, 0x21, 0x2a, 0x88, 0x00, 0x00, 0x00, 0x10}, * set by setfreq */
+/*      {0xa1, 0x21, 0x2b, 0x34, 0x00, 0x00, 0x00, 0x10}, * set by setfreq */
 /* */
         {0xa1, 0x21, 0x10, 0x83, 0x00, 0x00, 0x00, 0x10},
 /*      {0xb1, 0x21, 0x01, 0x88, 0x70, 0x00, 0x00, 0x10}, */
@@ -673,7 +700,7 @@ static const u8 ov7648_sensor_init[][8] = {
         {0xd1, 0x21, 0x21, 0x86, 0x00, 0xde, 0xa0, 0x10},
 /*      {0xd1, 0x21, 0x25, 0x80, 0x32, 0xfe, 0xa0, 0x10}, jfm done */
 /*      {0xd1, 0x21, 0x29, 0x00, 0x91, 0x00, 0x88, 0x10}, jfm done */
-        {0xb1, 0x21, 0x2d, 0x85, 0x00, 0x00, 0x00, 0x10},
+/*      {0xb1, 0x21, 0x2d, 0x85, 0x00, 0x00, 0x00, 0x10}, set by setfreq */
 /*...*/
 /*      {0xa1, 0x21, 0x12, 0x08, 0x00, 0x00, 0x00, 0x10}, jfm done */
 /*      {0xa1, 0x21, 0x75, 0x06, 0x00, 0x00, 0x00, 0x10},   * COMN
@@ -1294,11 +1321,9 @@ static int sd_config(struct gspca_dev *gspca_dev,
         sd->gamma = GAMMA_DEF;
         sd->autogain = AUTOGAIN_DEF;
         sd->ag_cnt = -1;
-        if (sd->sensor != SENSOR_OV7630)
-                sd->vflip = 0;
-        else
-                sd->vflip = 1;
+        sd->vflip = VFLIP_DEF;
         sd->infrared = INFRARED_DEF;
+        sd->freq = FREQ_DEF;
         sd->quality = QUALITY_DEF;
         sd->jpegqual = 80;
 
@@ -1569,7 +1594,7 @@ static void setautogain(struct gspca_dev *gspca_dev)
                 else
                         comb = 0xa0;
                 if (sd->autogain)
-                        comb |= 0x02;
+                        comb |= 0x03;
                 i2c_w1(&sd->gspca_dev, 0x13, comb);
                 return;
             }
@@ -1585,12 +1610,15 @@ static void setvflip(struct sd *sd)
 {
         u8 comn;
 
-        if (sd->sensor == SENSOR_OV7630)
+        if (sd->sensor == SENSOR_OV7630) {
                 comn = 0x02;
-        else
+                if (!sd->vflip)
+                        comn |= 0x80;
+        } else {
                 comn = 0x06;
-        if (sd->vflip)
-                comn |= 0x80;
+                if (sd->vflip)
+                        comn |= 0x80;
+        }
         i2c_w1(&sd->gspca_dev, 0x75, comn);
 }
 
@@ -1602,6 +1630,58 @@ static void setinfrared(struct sd *sd)
                 sd->infrared ? 0x66 : 0x64);
 }
 
+static void setfreq(struct gspca_dev *gspca_dev)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+
+        if (sd->sensor == SENSOR_OV7660) {
+                switch (sd->freq) {
+                case 0: /* Banding filter disabled */
+                        i2c_w1(gspca_dev, 0x13, 0xdf);
+                        break;
+                case 1: /* 50 hz */
+                        i2c_w1(gspca_dev, 0x13, 0xff);
+                        i2c_w1(gspca_dev, 0x3b, 0x0a);
+                        break;
+                case 2: /* 60 hz */
+                        i2c_w1(gspca_dev, 0x13, 0xff);
+                        i2c_w1(gspca_dev, 0x3b, 0x02);
+                        break;
+                }
+        } else {
+                u8 reg2a = 0, reg2b = 0, reg2d = 0;
+
+                /* Get reg2a / reg2d base values */
+                switch (sd->sensor) {
+                case SENSOR_OV7630:
+                        reg2a = 0x08;
+                        reg2d = 0x01;
+                        break;
+                case SENSOR_OV7648:
+                        reg2a = 0x11;
+                        reg2d = 0x81;
+                        break;
+                }
+
+                switch (sd->freq) {
+                case 0: /* Banding filter disabled */
+                        break;
+                case 1: /* 50 hz (filter on and framerate adj) */
+                        reg2a |= 0x80;
+                        reg2b = 0xac;
+                        reg2d |= 0x04;
+                        break;
+                case 2: /* 60 hz (filter on, no framerate adj) */
+                        reg2a |= 0x80;
+                        reg2d |= 0x04;
+                        break;
+                }
+                i2c_w1(gspca_dev, 0x2a, reg2a);
+                i2c_w1(gspca_dev, 0x2b, reg2b);
+                i2c_w1(gspca_dev, 0x2d, reg2d);
+        }
+}
+
 static void setjpegqual(struct gspca_dev *gspca_dev)
 {
         struct sd *sd = (struct sd *) gspca_dev;
@@ -1828,6 +1908,7 @@ static int sd_start(struct gspca_dev *gspca_dev)
         setbrightness(gspca_dev);
         setcontrast(gspca_dev);
         setautogain(gspca_dev);
+        setfreq(gspca_dev);
         return 0;
 }
 
@@ -2131,6 +2212,24 @@ static int sd_getinfrared(struct gspca_dev *gspca_dev, __s32 *val)
         return 0;
 }
 
+static int sd_setfreq(struct gspca_dev *gspca_dev, __s32 val)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+
+        sd->freq = val;
+        if (gspca_dev->streaming)
+                setfreq(gspca_dev);
+        return 0;
+}
+
+static int sd_getfreq(struct gspca_dev *gspca_dev, __s32 *val)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+
+        *val = sd->freq;
+        return 0;
+}
+
 static int sd_set_jcomp(struct gspca_dev *gspca_dev,
                         struct v4l2_jpegcompression *jcomp)
 {
@@ -2159,6 +2258,27 @@ static int sd_get_jcomp(struct gspca_dev *gspca_dev,
         return 0;
 }
 
+static int sd_querymenu(struct gspca_dev *gspca_dev,
+                        struct v4l2_querymenu *menu)
+{
+        switch (menu->id) {
+        case V4L2_CID_POWER_LINE_FREQUENCY:
+                switch (menu->index) {
+                case 0:         /* V4L2_CID_POWER_LINE_FREQUENCY_DISABLED */
+                        strcpy((char *) menu->name, "NoFliker");
+                        return 0;
+                case 1:         /* V4L2_CID_POWER_LINE_FREQUENCY_50HZ */
+                        strcpy((char *) menu->name, "50 Hz");
+                        return 0;
+                case 2:         /* V4L2_CID_POWER_LINE_FREQUENCY_60HZ */
+                        strcpy((char *) menu->name, "60 Hz");
+                        return 0;
+                }
+                break;
+        }
+        return -EINVAL;
+}
+
 /* sub-driver description */
 static const struct sd_desc sd_desc = {
         .name = MODULE_NAME,
@@ -2173,6 +2293,7 @@ static const struct sd_desc sd_desc = {
         .dq_callback = do_autogain,
         .get_jcomp = sd_get_jcomp,
         .set_jcomp = sd_set_jcomp,
+        .querymenu = sd_querymenu,
 };
 
 /* -- module initialisation -- */
@@ -2233,7 +2354,7 @@ static const __devinitdata struct usb_device_id device_table[] = {
         {USB_DEVICE(0x0c45, 0x613b), BSI(SN9C120, OV7660, 0x21)},
 #endif
         {USB_DEVICE(0x0c45, 0x613c), BSI(SN9C120, HV7131R, 0x11)},
-/*      {USB_DEVICE(0x0c45, 0x613e), BSI(SN9C120, OV7630, 0x??)}, */
+        {USB_DEVICE(0x0c45, 0x613e), BSI(SN9C120, OV7630, 0x21)},
         {USB_DEVICE(0x0c45, 0x6143), BSI(SN9C120, SP80708, 0x18)},
         {}
 };
diff --git a/drivers/media/video/gspca/stv06xx/Makefile b/drivers/media/video/gspca/stv06xx/Makefile
index feeaa94ab588..2f3c3a606ce4 100644
--- a/drivers/media/video/gspca/stv06xx/Makefile
+++ b/drivers/media/video/gspca/stv06xx/Makefile
@@ -3,7 +3,8 @@ obj-$(CONFIG_USB_STV06XX) += gspca_stv06xx.o
 gspca_stv06xx-objs := stv06xx.o \
                       stv06xx_vv6410.o \
                       stv06xx_hdcs.o \
-                      stv06xx_pb0100.o
+                      stv06xx_pb0100.o \
+                      stv06xx_st6422.o
 
 EXTRA_CFLAGS += -Idrivers/media/video/gspca
 
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx.c b/drivers/media/video/gspca/stv06xx/stv06xx.c
index e573c3406324..0da8e0de0456 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx.c
+++ b/drivers/media/video/gspca/stv06xx/stv06xx.c
@@ -92,11 +92,10 @@ static int stv06xx_write_sensor_finish(struct sd *sd)
 {
         int err = 0;
 
-        if (IS_850(sd)) {
+        if (sd->bridge == BRIDGE_STV610) {
                 struct usb_device *udev = sd->gspca_dev.dev;
                 __u8 *buf = sd->gspca_dev.usb_buf;
 
-                /* Quickam Web needs an extra packet */
                 buf[0] = 0;
                 err = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
                                       0x04, 0x40, 0x1704, 0, buf, 1,
@@ -253,7 +252,7 @@ static int stv06xx_init(struct gspca_dev *gspca_dev)
 
         err = sd->sensor->init(sd);
 
-        if (dump_sensor)
+        if (dump_sensor && sd->sensor->dump)
                 sd->sensor->dump(sd);
 
         return (err < 0) ? err : 0;
@@ -318,6 +317,8 @@ static void stv06xx_pkt_scan(struct gspca_dev *gspca_dev,
                         __u8 *data,                     /* isoc packet */
                         int len)                        /* iso packet length */
 {
+        struct sd *sd = (struct sd *) gspca_dev;
+
         PDEBUG(D_PACK, "Packet of length %d arrived", len);
 
         /* A packet may contain several frames
@@ -343,14 +344,29 @@ static void stv06xx_pkt_scan(struct gspca_dev *gspca_dev,
                 if (len < chunk_len) {
                         PDEBUG(D_ERR, "URB packet length is smaller"
                                 " than the specified chunk length");
+                        gspca_dev->last_packet_type = DISCARD_PACKET;
                         return;
                 }
 
+                /* First byte seem to be 02=data 2nd byte is unknown??? */
+                if (sd->bridge == BRIDGE_ST6422 && (id & 0xFF00) == 0x0200)
+                        goto frame_data;
+
                 switch (id) {
                 case 0x0200:
                 case 0x4200:
+frame_data:
                         PDEBUG(D_PACK, "Frame data packet detected");
 
+                        if (sd->to_skip) {
+                                int skip = (sd->to_skip < chunk_len) ?
+                                            sd->to_skip : chunk_len;
+                                data += skip;
+                                len -= skip;
+                                chunk_len -= skip;
+                                sd->to_skip -= skip;
+                        }
+
                         gspca_frame_add(gspca_dev, INTER_PACKET, frame,
                                         data, chunk_len);
                         break;
@@ -365,6 +381,9 @@ static void stv06xx_pkt_scan(struct gspca_dev *gspca_dev,
                         gspca_frame_add(gspca_dev, FIRST_PACKET,
                                         frame, data, 0);
 
+                        if (sd->bridge == BRIDGE_ST6422)
+                                sd->to_skip = gspca_dev->width * 4;
+
                         if (chunk_len)
                                 PDEBUG(D_ERR, "Chunk length is "
                                               "non-zero on a SOF");
@@ -395,8 +414,12 @@ static void stv06xx_pkt_scan(struct gspca_dev *gspca_dev,
                         /* Unknown chunk with 2 bytes of data,
                            occurs 2-3 times per USB interrupt */
                         break;
+                case 0x42ff:
+                        PDEBUG(D_PACK, "Chunk 0x42ff detected");
+                        /* Special chunk seen sometimes on the ST6422 */
+                        break;
                 default:
-                        PDEBUG(D_PACK, "Unknown chunk %d detected", id);
+                        PDEBUG(D_PACK, "Unknown chunk 0x%04x detected", id);
                         /* Unknown chunk */
                 }
                 data    += chunk_len;
@@ -428,11 +451,16 @@ static int stv06xx_config(struct gspca_dev *gspca_dev,
 
         cam = &gspca_dev->cam;
         sd->desc = sd_desc;
+        sd->bridge = id->driver_info;
         gspca_dev->sd_desc = &sd->desc;
 
         if (dump_bridge)
                 stv06xx_dump_bridge(sd);
 
+        sd->sensor = &stv06xx_sensor_st6422;
+        if (!sd->sensor->probe(sd))
+                return 0;
+
         sd->sensor = &stv06xx_sensor_vv6410;
         if (!sd->sensor->probe(sd))
                 return 0;
@@ -457,9 +485,20 @@ static int stv06xx_config(struct gspca_dev *gspca_dev,
 
 /* -- module initialisation -- */
 static const __devinitdata struct usb_device_id device_table[] = {
-        {USB_DEVICE(0x046d, 0x0840)}, /* QuickCam Express */
-        {USB_DEVICE(0x046d, 0x0850)}, /* LEGO cam / QuickCam Web */
-        {USB_DEVICE(0x046d, 0x0870)}, /* Dexxa WebCam USB */
+        /* QuickCam Express */
+        {USB_DEVICE(0x046d, 0x0840), .driver_info = BRIDGE_STV600 },
+        /* LEGO cam / QuickCam Web */
+        {USB_DEVICE(0x046d, 0x0850), .driver_info = BRIDGE_STV610 },
+        /* Dexxa WebCam USB */
+        {USB_DEVICE(0x046d, 0x0870), .driver_info = BRIDGE_STV602 },
+        /* QuickCam Messenger */
+        {USB_DEVICE(0x046D, 0x08F0), .driver_info = BRIDGE_ST6422 },
+        /* QuickCam Communicate */
+        {USB_DEVICE(0x046D, 0x08F5), .driver_info = BRIDGE_ST6422 },
+        /* QuickCam Messenger (new) */
+        {USB_DEVICE(0x046D, 0x08F6), .driver_info = BRIDGE_ST6422 },
+        /* QuickCam Messenger (new) */
+        {USB_DEVICE(0x046D, 0x08DA), .driver_info = BRIDGE_ST6422 },
         {}
 };
 MODULE_DEVICE_TABLE(usb, device_table);
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx.h b/drivers/media/video/gspca/stv06xx/stv06xx.h
index 1207e7d17f14..9df7137fe67e 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx.h
@@ -93,6 +93,17 @@ struct sd {
 
         /* Sensor private data */
         void *sensor_priv;
+
+        /* The first 4 lines produced by the stv6422 are no good, this keeps
+           track of how many bytes we still need to skip during a frame */
+        int to_skip;
+
+        /* Bridge / Camera type */
+        u8 bridge;
+        #define BRIDGE_STV600 0
+        #define BRIDGE_STV602 1
+        #define BRIDGE_STV610 2
+        #define BRIDGE_ST6422 3 /* With integrated sensor */
 };
 
 int stv06xx_write_bridge(struct sd *sd, u16 address, u16 i2c_data);
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c b/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c
index b16903814203..3039ec208f3a 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_hdcs.c
@@ -434,7 +434,7 @@ static int hdcs_probe_1x00(struct sd *sd)
         hdcs->exp.er = 100;
 
         /*
-         * Frame rate on HDCS-1000 0x46D:0x840 depends on PSMP:
+         * Frame rate on HDCS-1000 with STV600 depends on PSMP:
          *  4 = doesn't work at all
          *  5 = 7.8 fps,
          *  6 = 6.9 fps,
@@ -443,7 +443,7 @@ static int hdcs_probe_1x00(struct sd *sd)
          * 15 = 4.4 fps,
          * 31 = 2.8 fps
          *
-         * Frame rate on HDCS-1000 0x46D:0x870 depends on PSMP:
+         * Frame rate on HDCS-1000 with STV602 depends on PSMP:
          * 15 = doesn't work at all
          * 18 = doesn't work at all
          * 19 = 7.3 fps
@@ -453,7 +453,7 @@ static int hdcs_probe_1x00(struct sd *sd)
          * 24 = 6.3 fps
          * 30 = 5.4 fps
          */
-        hdcs->psmp = IS_870(sd) ? 20 : 5;
+        hdcs->psmp = (sd->bridge == BRIDGE_STV602) ? 20 : 5;
 
         sd->sensor_priv = hdcs;
 
@@ -530,7 +530,7 @@ static int hdcs_init(struct sd *sd)
         int i, err = 0;
 
         /* Set the STV0602AA in STV0600 emulation mode */
-        if (IS_870(sd))
+        if (sd->bridge == BRIDGE_STV602)
                 stv06xx_write_bridge(sd, STV_STV0600_EMULATION, 1);
 
         /* Execute the bridge init */
@@ -558,7 +558,7 @@ static int hdcs_init(struct sd *sd)
                 return err;
 
         /* Set PGA sample duration
-        (was 0x7E for IS_870, but caused slow framerate with HDCS-1020) */
+        (was 0x7E for the STV602, but caused slow framerate with HDCS-1020) */
         if (IS_1020(sd))
                 err = stv06xx_write_sensor(sd, HDCS_TCTRL,
                                 (HDCS_ADC_START_SIG_DUR << 6) | hdcs->psmp);
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h b/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h
index e88c42f7d2f8..934b9cebc1ab 100644
--- a/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_sensor.h
@@ -32,14 +32,13 @@
 
 #include "stv06xx.h"
 
-#define IS_850(sd)      ((sd)->gspca_dev.dev->descriptor.idProduct == 0x850)
-#define IS_870(sd)      ((sd)->gspca_dev.dev->descriptor.idProduct == 0x870)
 #define IS_1020(sd)     ((sd)->sensor == &stv06xx_sensor_hdcs1020)
 
 extern const struct stv06xx_sensor stv06xx_sensor_vv6410;
 extern const struct stv06xx_sensor stv06xx_sensor_hdcs1x00;
 extern const struct stv06xx_sensor stv06xx_sensor_hdcs1020;
 extern const struct stv06xx_sensor stv06xx_sensor_pb0100;
+extern const struct stv06xx_sensor stv06xx_sensor_st6422;
 
 struct stv06xx_sensor {
         /* Defines the name of a sensor */
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_st6422.c b/drivers/media/video/gspca/stv06xx/stv06xx_st6422.c
new file mode 100644
index 000000000000..87cb5b9ddfa7
--- /dev/null
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_st6422.c
@@ -0,0 +1,453 @@
+/*
+ * Support for the sensor part which is integrated (I think) into the
+ * st6422 stv06xx alike bridge, as its integrated there are no i2c writes
+ * but instead direct bridge writes.
+ *
+ * Copyright (c) 2009 Hans de Goede <hdegoede@redhat.com>
+ *
+ * Strongly based on qc-usb-messenger, which is:
+ * Copyright (c) 2001 Jean-Fredric Clere, Nikolas Zimmermann, Georg Acher
+ *                    Mark Cave-Ayland, Carlo E Prelz, Dick Streefland
+ * Copyright (c) 2002, 2003 Tuukka Toivonen
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#include "stv06xx_st6422.h"
+
+static struct v4l2_pix_format st6422_mode[] = {
+        /* Note we actually get 124 lines of data, of which we skip the 4st
+           4 as they are garbage */
+        {
+                162,
+                120,
+                V4L2_PIX_FMT_SGRBG8,
+                V4L2_FIELD_NONE,
+                .sizeimage = 162 * 120,
+                .bytesperline = 162,
+                .colorspace = V4L2_COLORSPACE_SRGB,
+                .priv = 1
+        },
+        /* Note we actually get 248 lines of data, of which we skip the 4st
+           4 as they are garbage, and we tell the app it only gets the
+           first 240 of the 244 lines it actually gets, so that it ignores
+           the last 4. */
+        {
+                324,
+                240,
+                V4L2_PIX_FMT_SGRBG8,
+                V4L2_FIELD_NONE,
+                .sizeimage = 324 * 244,
+                .bytesperline = 324,
+                .colorspace = V4L2_COLORSPACE_SRGB,
+                .priv = 0
+        },
+};
+
+static const struct ctrl st6422_ctrl[] = {
+#define BRIGHTNESS_IDX 0
+        {
+                {
+                        .id             = V4L2_CID_BRIGHTNESS,
+                        .type           = V4L2_CTRL_TYPE_INTEGER,
+                        .name           = "Brightness",
+                        .minimum        = 0,
+                        .maximum        = 31,
+                        .step           = 1,
+                        .default_value  = 3
+                },
+                .set = st6422_set_brightness,
+                .get = st6422_get_brightness
+        },
+#define CONTRAST_IDX 1
+        {
+                {
+                        .id             = V4L2_CID_CONTRAST,
+                        .type           = V4L2_CTRL_TYPE_INTEGER,
+                        .name           = "Contrast",
+                        .minimum        = 0,
+                        .maximum        = 15,
+                        .step           = 1,
+                        .default_value  = 11
+                },
+                .set = st6422_set_contrast,
+                .get = st6422_get_contrast
+        },
+#define GAIN_IDX 2
+        {
+                {
+                        .id             = V4L2_CID_GAIN,
+                        .type           = V4L2_CTRL_TYPE_INTEGER,
+                        .name           = "Gain",
+                        .minimum        = 0,
+                        .maximum        = 255,
+                        .step           = 1,
+                        .default_value  = 64
+                },
+                .set = st6422_set_gain,
+                .get = st6422_get_gain
+        },
+#define EXPOSURE_IDX 3
+        {
+                {
+                        .id             = V4L2_CID_EXPOSURE,
+                        .type           = V4L2_CTRL_TYPE_INTEGER,
+                        .name           = "Exposure",
+                        .minimum        = 0,
+                        .maximum        = 1023,
+                        .step           = 1,
+                        .default_value  = 256
+                },
+                .set = st6422_set_exposure,
+                .get = st6422_get_exposure
+        },
+};
+
+static int st6422_probe(struct sd *sd)
+{
+        int i;
+        s32 *sensor_settings;
+
+        if (sd->bridge != BRIDGE_ST6422)
+                return -ENODEV;
+
+        info("st6422 sensor detected");
+
+        sensor_settings = kmalloc(ARRAY_SIZE(st6422_ctrl) * sizeof(s32),
+                                  GFP_KERNEL);
+        if (!sensor_settings)
+                return -ENOMEM;
+
+        sd->gspca_dev.cam.cam_mode = st6422_mode;
+        sd->gspca_dev.cam.nmodes = ARRAY_SIZE(st6422_mode);
+        sd->desc.ctrls = st6422_ctrl;
+        sd->desc.nctrls = ARRAY_SIZE(st6422_ctrl);
+        sd->sensor_priv = sensor_settings;
+
+        for (i = 0; i < sd->desc.nctrls; i++)
+                sensor_settings[i] = st6422_ctrl[i].qctrl.default_value;
+
+        return 0;
+}
+
+static int st6422_init(struct sd *sd)
+{
+        int err = 0, i;
+
+        const u16 st6422_bridge_init[][2] = {
+                { STV_ISO_ENABLE, 0x00 }, /* disable capture */
+                { 0x1436, 0x00 },
+                { 0x1432, 0x03 },       /* 0x00-0x1F brightness */
+                { 0x143a, 0xF9 },       /* 0x00-0x0F contrast */
+                { 0x0509, 0x38 },       /* R */
+                { 0x050a, 0x38 },       /* G */
+                { 0x050b, 0x38 },       /* B */
+                { 0x050c, 0x2A },
+                { 0x050d, 0x01 },
+
+
+                { 0x1431, 0x00 },       /* 0x00-0x07 ??? */
+                { 0x1433, 0x34 },       /* 160x120, 0x00-0x01 night filter */
+                { 0x1438, 0x18 },       /* 640x480 */
+/* 18 bayes */
+/* 10 compressed? */
+
+                { 0x1439, 0x00 },
+/* antiflimmer??  0xa2 ger perfekt bild mot monitor */
+
+                { 0x143b, 0x05 },
+                { 0x143c, 0x00 },       /* 0x00-0x01 - ??? */
+
+
+/* shutter time 0x0000-0x03FF */
+/* low value  give good picures on moving objects (but requires much light) */
+/* high value gives good picures in darkness (but tends to be overexposed) */
+                { 0x143e, 0x01 },
+                { 0x143d, 0x00 },
+
+                { 0x1442, 0xe2 },
+/* write: 1x1x xxxx */
+/* read:  1x1x xxxx */
+/*        bit 5 == button pressed and hold if 0 */
+/* write 0xe2,0xea */
+
+/* 0x144a */
+/* 0x00 init */
+/* bit 7 == button has been pressed, but not handled */
+
+/* interrupt */
+/* if(urb->iso_frame_desc[i].status == 0x80) { */
+/* if(urb->iso_frame_desc[i].status == 0x88) { */
+
+                { 0x1500, 0xd0 },
+                { 0x1500, 0xd0 },
+                { 0x1500, 0x50 },       /* 0x00 - 0xFF  0x80 == compr ? */
+
+                { 0x1501, 0xaf },
+/* high val-> ljus area blir morkare. */
+/* low val -> ljus area blir ljusare. */
+                { 0x1502, 0xc2 },
+/* high val-> ljus area blir morkare. */
+/* low val -> ljus area blir ljusare. */
+                { 0x1503, 0x45 },
+/* high val-> ljus area blir morkare. */
+/* low val -> ljus area blir ljusare. */
+
+                { 0x1505, 0x02 },
+/* 2  : 324x248  80352 bytes */
+/* 7  : 248x162  40176 bytes */
+/* c+f: 162*124  20088 bytes */
+
+                { 0x150e, 0x8e },
+                { 0x150f, 0x37 },
+                { 0x15c0, 0x00 },
+                { 0x15c1, 1023 }, /* 160x120, ISOC_PACKET_SIZE */
+                { 0x15c3, 0x08 },       /* 0x04/0x14 ... test pictures ??? */
+
+
+                { 0x143f, 0x01 },       /* commit settings */
+
+        };
+
+        for (i = 0; i < ARRAY_SIZE(st6422_bridge_init) && !err; i++) {
+                err = stv06xx_write_bridge(sd, st6422_bridge_init[i][0],
+                                               st6422_bridge_init[i][1]);
+        }
+
+        return err;
+}
+
+static void st6422_disconnect(struct sd *sd)
+{
+        sd->sensor = NULL;
+        kfree(sd->sensor_priv);
+}
+
+static int st6422_start(struct sd *sd)
+{
+        int err, packet_size;
+        struct cam *cam = &sd->gspca_dev.cam;
+        s32 *sensor_settings = sd->sensor_priv;
+        struct usb_host_interface *alt;
+        struct usb_interface *intf;
+
+        intf = usb_ifnum_to_if(sd->gspca_dev.dev, sd->gspca_dev.iface);
+        alt = usb_altnum_to_altsetting(intf, sd->gspca_dev.alt);
+        if (!alt) {
+                PDEBUG(D_ERR, "Couldn't get altsetting");
+                return -EIO;
+        }
+
+        packet_size = le16_to_cpu(alt->endpoint[0].desc.wMaxPacketSize);
+        err = stv06xx_write_bridge(sd, 0x15c1, packet_size);
+        if (err < 0)
+                return err;
+
+        if (cam->cam_mode[sd->gspca_dev.curr_mode].priv)
+                err = stv06xx_write_bridge(sd, 0x1505, 0x0f);
+        else
+                err = stv06xx_write_bridge(sd, 0x1505, 0x02);
+        if (err < 0)
+                return err;
+
+        err = st6422_set_brightness(&sd->gspca_dev,
+                                    sensor_settings[BRIGHTNESS_IDX]);
+        if (err < 0)
+                return err;
+
+        err = st6422_set_contrast(&sd->gspca_dev,
+                                  sensor_settings[CONTRAST_IDX]);
+        if (err < 0)
+                return err;
+
+        err = st6422_set_exposure(&sd->gspca_dev,
+                                  sensor_settings[EXPOSURE_IDX]);
+        if (err < 0)
+                return err;
+
+        err = st6422_set_gain(&sd->gspca_dev,
+                              sensor_settings[GAIN_IDX]);
+        if (err < 0)
+                return err;
+
+        PDEBUG(D_STREAM, "Starting stream");
+
+        return 0;
+}
+
+static int st6422_stop(struct sd *sd)
+{
+        PDEBUG(D_STREAM, "Halting stream");
+
+        return 0;
+}
+
+static int st6422_get_brightness(struct gspca_dev *gspca_dev, __s32 *val)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+        s32 *sensor_settings = sd->sensor_priv;
+
+        *val = sensor_settings[BRIGHTNESS_IDX];
+
+        PDEBUG(D_V4L2, "Read brightness %d", *val);
+
+        return 0;
+}
+
+static int st6422_set_brightness(struct gspca_dev *gspca_dev, __s32 val)
+{
+        int err;
+        struct sd *sd = (struct sd *) gspca_dev;
+        s32 *sensor_settings = sd->sensor_priv;
+
+        sensor_settings[BRIGHTNESS_IDX] = val;
+
+        if (!gspca_dev->streaming)
+                return 0;
+
+        /* val goes from 0 -> 31 */
+        PDEBUG(D_V4L2, "Set brightness to %d", val);
+        err = stv06xx_write_bridge(sd, 0x1432, val);
+        if (err < 0)
+                return err;
+
+        /* commit settings */
+        err = stv06xx_write_bridge(sd, 0x143f, 0x01);
+        return (err < 0) ? err : 0;
+}
+
+static int st6422_get_contrast(struct gspca_dev *gspca_dev, __s32 *val)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+        s32 *sensor_settings = sd->sensor_priv;
+
+        *val = sensor_settings[CONTRAST_IDX];
+
+        PDEBUG(D_V4L2, "Read contrast %d", *val);
+
+        return 0;
+}
+
+static int st6422_set_contrast(struct gspca_dev *gspca_dev, __s32 val)
+{
+        int err;
+        struct sd *sd = (struct sd *) gspca_dev;
+        s32 *sensor_settings = sd->sensor_priv;
+
+        sensor_settings[CONTRAST_IDX] = val;
+
+        if (!gspca_dev->streaming)
+                return 0;
+
+        /* Val goes from 0 -> 15 */
+        PDEBUG(D_V4L2, "Set contrast to %d\n", val);
+        err = stv06xx_write_bridge(sd, 0x143a, 0xf0 | val);
+        if (err < 0)
+                return err;
+
+        /* commit settings */
+        err = stv06xx_write_bridge(sd, 0x143f, 0x01);
+        return (err < 0) ? err : 0;
+}
+
+static int st6422_get_gain(struct gspca_dev *gspca_dev, __s32 *val)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+        s32 *sensor_settings = sd->sensor_priv;
+
+        *val = sensor_settings[GAIN_IDX];
+
+        PDEBUG(D_V4L2, "Read gain %d", *val);
+
+        return 0;
+}
+
+static int st6422_set_gain(struct gspca_dev *gspca_dev, __s32 val)
+{
+        int err;
+        struct sd *sd = (struct sd *) gspca_dev;
+        s32 *sensor_settings = sd->sensor_priv;
+
+        sensor_settings[GAIN_IDX] = val;
+
+        if (!gspca_dev->streaming)
+                return 0;
+
+        PDEBUG(D_V4L2, "Set gain to %d", val);
+
+        /* Set red, green, blue, gain */
+        err = stv06xx_write_bridge(sd, 0x0509, val);
+        if (err < 0)
+                return err;
+
+        err = stv06xx_write_bridge(sd, 0x050a, val);
+        if (err < 0)
+                return err;
+
+        err = stv06xx_write_bridge(sd, 0x050b, val);
+        if (err < 0)
+                return err;
+
+        /* 2 mystery writes */
+        err = stv06xx_write_bridge(sd, 0x050c, 0x2a);
+        if (err < 0)
+                return err;
+
+        err = stv06xx_write_bridge(sd, 0x050d, 0x01);
+        if (err < 0)
+                return err;
+
+        /* commit settings */
+        err = stv06xx_write_bridge(sd, 0x143f, 0x01);
+        return (err < 0) ? err : 0;
+}
+
+static int st6422_get_exposure(struct gspca_dev *gspca_dev, __s32 *val)
+{
+        struct sd *sd = (struct sd *) gspca_dev;
+        s32 *sensor_settings = sd->sensor_priv;
+
+        *val = sensor_settings[EXPOSURE_IDX];
+
+        PDEBUG(D_V4L2, "Read exposure %d", *val);
+
+        return 0;
+}
+
+static int st6422_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
+{
+        int err;
+        struct sd *sd = (struct sd *) gspca_dev;
+        s32 *sensor_settings = sd->sensor_priv;
+
+        sensor_settings[EXPOSURE_IDX] = val;
+
+        if (!gspca_dev->streaming)
+                return 0;
+
+        PDEBUG(D_V4L2, "Set exposure to %d\n", val);
+        err = stv06xx_write_bridge(sd, 0x143d, val & 0xff);
+        if (err < 0)
+                return err;
+
+        err = stv06xx_write_bridge(sd, 0x143e, val >> 8);
+        if (err < 0)
+                return err;
+
+        /* commit settings */
+        err = stv06xx_write_bridge(sd, 0x143f, 0x01);
+        return (err < 0) ? err : 0;
+}
diff --git a/drivers/media/video/gspca/stv06xx/stv06xx_st6422.h b/drivers/media/video/gspca/stv06xx/stv06xx_st6422.h
new file mode 100644
index 000000000000..b2d45fe50522
--- /dev/null
+++ b/drivers/media/video/gspca/stv06xx/stv06xx_st6422.h
@@ -0,0 +1,59 @@
+/*
+ * Support for the sensor part which is integrated (I think) into the
+ * st6422 stv06xx alike bridge, as its integrated there are no i2c writes
+ * but instead direct bridge writes.
+ *
+ * Copyright (c) 2009 Hans de Goede <hdegoede@redhat.com>
+ *
+ * Strongly based on qc-usb-messenger, which is:
+ * Copyright (c) 2001 Jean-Fredric Clere, Nikolas Zimmermann, Georg Acher
+ *                    Mark Cave-Ayland, Carlo E Prelz, Dick Streefland
+ * Copyright (c) 2002, 2003 Tuukka Toivonen
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ *
+ */
+
+#ifndef STV06XX_ST6422_H_
+#define STV06XX_ST6422_H_
+
+#include "stv06xx_sensor.h"
+
+static int st6422_probe(struct sd *sd);
+static int st6422_start(struct sd *sd);
+static int st6422_init(struct sd *sd);
+static int st6422_stop(struct sd *sd);
+static void st6422_disconnect(struct sd *sd);
+
+/* V4L2 controls supported by the driver */
+static int st6422_get_brightness(struct gspca_dev *gspca_dev, __s32 *val);
+static int st6422_set_brightness(struct gspca_dev *gspca_dev, __s32 val);
+static int st6422_get_contrast(struct gspca_dev *gspca_dev, __s32 *val);
+static int st6422_set_contrast(struct gspca_dev *gspca_dev, __s32 val);
+static int st6422_get_gain(struct gspca_dev *gspca_dev, __s32 *val);
+static int st6422_set_gain(struct gspca_dev *gspca_dev, __s32 val);
+static int st6422_get_exposure(struct gspca_dev *gspca_dev, __s32 *val);
+static int st6422_set_exposure(struct gspca_dev *gspca_dev, __s32 val);
+
+const struct stv06xx_sensor stv06xx_sensor_st6422 = {
+        .name = "ST6422",
+        .init = st6422_init,
+        .probe = st6422_probe,
+        .start = st6422_start,
+        .stop = st6422_stop,
+        .disconnect = st6422_disconnect,
+};
+
+#endif
diff --git a/drivers/media/video/ivtv/ivtv-controls.c b/drivers/media/video/ivtv/ivtv-controls.c
index 84995bcf4a75..a3b77ed3f089 100644
--- a/drivers/media/video/ivtv/ivtv-controls.c
+++ b/drivers/media/video/ivtv/ivtv-controls.c
@@ -60,6 +60,8 @@ int ivtv_queryctrl(struct file *file, void *fh, struct v4l2_queryctrl *qctrl)
 
         switch (qctrl->id) {
         /* Standard V4L2 controls */
+        case V4L2_CID_USER_CLASS:
+                return v4l2_ctrl_query_fill(qctrl, 0, 0, 0, 0);
         case V4L2_CID_BRIGHTNESS:
         case V4L2_CID_HUE:
         case V4L2_CID_SATURATION:
diff --git a/drivers/media/video/mt9m001.c b/drivers/media/video/mt9m001.c
index 459c04cbf69d..4d794b42d6cd 100644
--- a/drivers/media/video/mt9m001.c
+++ b/drivers/media/video/mt9m001.c
@@ -280,15 +280,9 @@ static int mt9m001_try_fmt(struct soc_camera_device *icd,
 {
         struct v4l2_pix_format *pix = &f->fmt.pix;
 
-        if (pix->height < 32 + icd->y_skip_top)
-                pix->height = 32 + icd->y_skip_top;
-        if (pix->height > 1024 + icd->y_skip_top)
-                pix->height = 1024 + icd->y_skip_top;
-        if (pix->width < 48)
-                pix->width = 48;
-        if (pix->width > 1280)
-                pix->width = 1280;
-        pix->width &= ~0x01; /* has to be even, unsure why was ~3 */
+        v4l_bound_align_image(&pix->width, 48, 1280, 1,
+                              &pix->height, 32 + icd->y_skip_top,
+                              1024 + icd->y_skip_top, 0, 0);
 
         return 0;
 }
diff --git a/drivers/media/video/mt9t031.c b/drivers/media/video/mt9t031.c
index f72aeb7c4deb..4207fb342670 100644
--- a/drivers/media/video/mt9t031.c
+++ b/drivers/media/video/mt9t031.c
@@ -385,17 +385,9 @@ static int mt9t031_try_fmt(struct soc_camera_device *icd,
 {
         struct v4l2_pix_format *pix = &f->fmt.pix;
 
-        if (pix->height < MT9T031_MIN_HEIGHT)
-                pix->height = MT9T031_MIN_HEIGHT;
-        if (pix->height > MT9T031_MAX_HEIGHT)
-                pix->height = MT9T031_MAX_HEIGHT;
-        if (pix->width < MT9T031_MIN_WIDTH)
-                pix->width = MT9T031_MIN_WIDTH;
-        if (pix->width > MT9T031_MAX_WIDTH)
-                pix->width = MT9T031_MAX_WIDTH;
-
-        pix->width &= ~0x01; /* has to be even */
-        pix->height &= ~0x01; /* has to be even */
+        v4l_bound_align_image(
+                &pix->width, MT9T031_MIN_WIDTH, MT9T031_MAX_WIDTH, 1,
+                &pix->height, MT9T031_MIN_HEIGHT, MT9T031_MAX_HEIGHT, 1, 0);
 
         return 0;
 }
diff --git a/drivers/media/video/mt9v022.c b/drivers/media/video/mt9v022.c
index be20d312b1dc..dbdcc86ae50d 100644
--- a/drivers/media/video/mt9v022.c
+++ b/drivers/media/video/mt9v022.c
@@ -364,15 +364,9 @@ static int mt9v022_try_fmt(struct soc_camera_device *icd,
 {
         struct v4l2_pix_format *pix = &f->fmt.pix;
 
-        if (pix->height < 32 + icd->y_skip_top)
-                pix->height = 32 + icd->y_skip_top;
-        if (pix->height > 480 + icd->y_skip_top)
-                pix->height = 480 + icd->y_skip_top;
-        if (pix->width < 48)
-                pix->width = 48;
-        if (pix->width > 752)
-                pix->width = 752;
-        pix->width &= ~0x03; /* ? */
+        v4l_bound_align_image(&pix->width, 48, 752, 2 /* ? */,
+                              &pix->height, 32 + icd->y_skip_top,
+                              480 + icd->y_skip_top, 0, 0);
 
         return 0;
 }
diff --git a/drivers/media/video/ov511.c b/drivers/media/video/ov511.c
index 08cfd3e4ae8a..0bc2cf573c76 100644
--- a/drivers/media/video/ov511.c
+++ b/drivers/media/video/ov511.c
@@ -211,8 +211,6 @@ static const int i2c_detect_tries = 5;
 static struct usb_device_id device_table [] = {
         { USB_DEVICE(VEND_OMNIVISION, PROD_OV511) },
         { USB_DEVICE(VEND_OMNIVISION, PROD_OV511PLUS) },
-        { USB_DEVICE(VEND_OMNIVISION, PROD_OV518) },
-        { USB_DEVICE(VEND_OMNIVISION, PROD_OV518PLUS) },
         { USB_DEVICE(VEND_MATTEL, PROD_ME2CAM) },
         { }  /* Terminating entry */
 };
diff --git a/drivers/media/video/pvrusb2/pvrusb2-audio.c b/drivers/media/video/pvrusb2/pvrusb2-audio.c
index 10ef1a2c13ea..416933ca607d 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-audio.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-audio.c
@@ -48,11 +48,13 @@ static const int routing_scheme0[] = {
                                                 MSP_DSP_IN_SCART),
 };
 
-static const struct routing_scheme routing_schemes[] = {
-        [PVR2_ROUTING_SCHEME_HAUPPAUGE] = {
-                .def = routing_scheme0,
-                .cnt = ARRAY_SIZE(routing_scheme0),
-        },
+static const struct routing_scheme routing_def0 = {
+        .def = routing_scheme0,
+        .cnt = ARRAY_SIZE(routing_scheme0),
+};
+
+static const struct routing_scheme *routing_schemes[] = {
+        [PVR2_ROUTING_SCHEME_HAUPPAUGE] = &routing_def0,
 };
 
 void pvr2_msp3400_subdev_update(struct pvr2_hdw *hdw, struct v4l2_subdev *sd)
@@ -65,7 +67,7 @@ void pvr2_msp3400_subdev_update(struct pvr2_hdw *hdw, struct v4l2_subdev *sd)
                 pvr2_trace(PVR2_TRACE_CHIPS, "subdev msp3400 v4l2 set_stereo");
 
                 if ((sid < ARRAY_SIZE(routing_schemes)) &&
-                    ((sp = routing_schemes + sid) != NULL) &&
+                    ((sp = routing_schemes[sid]) != NULL) &&
                     (hdw->input_val >= 0) &&
                     (hdw->input_val < sp->cnt)) {
                         input = sp->def[hdw->input_val];
diff --git a/drivers/media/video/pvrusb2/pvrusb2-cs53l32a.c b/drivers/media/video/pvrusb2/pvrusb2-cs53l32a.c
index 9023adf3fdcc..68980e19409f 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-cs53l32a.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-cs53l32a.c
@@ -49,11 +49,13 @@ static const int routing_scheme1[] = {
         [PVR2_CVAL_INPUT_SVIDEO] =  0,
 };
 
-static const struct routing_scheme routing_schemes[] = {
-        [PVR2_ROUTING_SCHEME_ONAIR] = {
-                .def = routing_scheme1,
-                .cnt = ARRAY_SIZE(routing_scheme1),
-        },
+static const struct routing_scheme routing_def1 = {
+        .def = routing_scheme1,
+        .cnt = ARRAY_SIZE(routing_scheme1),
+};
+
+static const struct routing_scheme *routing_schemes[] = {
+        [PVR2_ROUTING_SCHEME_ONAIR] = &routing_def1,
 };
 
 
@@ -65,12 +67,11 @@ void pvr2_cs53l32a_subdev_update(struct pvr2_hdw *hdw, struct v4l2_subdev *sd)
                 u32 input;
                 pvr2_trace(PVR2_TRACE_CHIPS, "subdev v4l2 set_input(%d)",
                            hdw->input_val);
-                if ((sid < ARRAY_SIZE(routing_schemes)) &&
-                    ((sp = routing_schemes + sid) != NULL) &&
-                    (hdw->input_val >= 0) &&
-                    (hdw->input_val < sp->cnt)) {
-                        input = sp->def[hdw->input_val];
-                } else {
+                sp = (sid < ARRAY_SIZE(routing_schemes)) ?
+                        routing_schemes[sid] : NULL;
+                if ((sp == NULL) ||
+                    (hdw->input_val < 0) ||
+                    (hdw->input_val >= sp->cnt)) {
                         pvr2_trace(PVR2_TRACE_ERROR_LEGS,
                                    "*** WARNING *** subdev v4l2 set_input:"
                                    " Invalid routing scheme (%u)"
@@ -78,6 +79,7 @@ void pvr2_cs53l32a_subdev_update(struct pvr2_hdw *hdw, struct v4l2_subdev *sd)
                                    sid, hdw->input_val);
                         return;
                 }
+                input = sp->def[hdw->input_val];
                 sd->ops->audio->s_routing(sd, input, 0, 0);
         }
 }
diff --git a/drivers/media/video/pvrusb2/pvrusb2-cx2584x-v4l.c b/drivers/media/video/pvrusb2/pvrusb2-cx2584x-v4l.c
index 05e52358ae49..82c135835753 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-cx2584x-v4l.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-cx2584x-v4l.c
@@ -68,6 +68,11 @@ static const struct routing_scheme_item routing_scheme0[] = {
         },
 };
 
+static const struct routing_scheme routing_def0 = {
+        .def = routing_scheme0,
+        .cnt = ARRAY_SIZE(routing_scheme0),
+};
+
 /* Specific to gotview device */
 static const struct routing_scheme_item routing_schemegv[] = {
         [PVR2_CVAL_INPUT_TV] = {
@@ -90,15 +95,14 @@ static const struct routing_scheme_item routing_schemegv[] = {
         },
 };
 
-static const struct routing_scheme routing_schemes[] = {
-        [PVR2_ROUTING_SCHEME_HAUPPAUGE] = {
-                .def = routing_scheme0,
-                .cnt = ARRAY_SIZE(routing_scheme0),
-        },
-        [PVR2_ROUTING_SCHEME_GOTVIEW] = {
-                .def = routing_schemegv,
-                .cnt = ARRAY_SIZE(routing_schemegv),
-        },
+static const struct routing_scheme routing_defgv = {
+        .def = routing_schemegv,
+        .cnt = ARRAY_SIZE(routing_schemegv),
+};
+
+static const struct routing_scheme *routing_schemes[] = {
+        [PVR2_ROUTING_SCHEME_HAUPPAUGE] = &routing_def0,
+        [PVR2_ROUTING_SCHEME_GOTVIEW] = &routing_defgv,
 };
 
 void pvr2_cx25840_subdev_update(struct pvr2_hdw *hdw, struct v4l2_subdev *sd)
@@ -110,13 +114,11 @@ void pvr2_cx25840_subdev_update(struct pvr2_hdw *hdw, struct v4l2_subdev *sd)
                 const struct routing_scheme *sp;
                 unsigned int sid = hdw->hdw_desc->signal_routing_scheme;
 
-                if ((sid < ARRAY_SIZE(routing_schemes)) &&
-                    ((sp = routing_schemes + sid) != NULL) &&
-                    (hdw->input_val >= 0) &&
-                    (hdw->input_val < sp->cnt)) {
-                        vid_input = sp->def[hdw->input_val].vid;
-                        aud_input = sp->def[hdw->input_val].aud;
-                } else {
+                sp = (sid < ARRAY_SIZE(routing_schemes)) ?
+                        routing_schemes[sid] : NULL;
+                if ((sp == NULL) ||
+                    (hdw->input_val < 0) ||
+                    (hdw->input_val >= sp->cnt)) {
                         pvr2_trace(PVR2_TRACE_ERROR_LEGS,
                                    "*** WARNING *** subdev cx2584x set_input:"
                                    " Invalid routing scheme (%u)"
@@ -124,7 +126,8 @@ void pvr2_cx25840_subdev_update(struct pvr2_hdw *hdw, struct v4l2_subdev *sd)
                                    sid, hdw->input_val);
                         return;
                 }
-
+                vid_input = sp->def[hdw->input_val].vid;
+                aud_input = sp->def[hdw->input_val].aud;
                 pvr2_trace(PVR2_TRACE_CHIPS,
                            "subdev cx2584x set_input vid=0x%x aud=0x%x",
                            vid_input, aud_input);
diff --git a/drivers/media/video/pvrusb2/pvrusb2-hdw.c b/drivers/media/video/pvrusb2/pvrusb2-hdw.c
index 0c745b142fb7..cbc388729d77 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-hdw.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-hdw.c
@@ -85,8 +85,8 @@ MODULE_PARM_DESC(video_std,"specify initial video standard");
 module_param_array(tolerance,    int, NULL, 0444);
 MODULE_PARM_DESC(tolerance,"specify stream error tolerance");
 
-/* US Broadcast channel 7 (175.25 MHz) */
-static int default_tv_freq    = 175250000L;
+/* US Broadcast channel 3 (61.25 MHz), to help with testing */
+static int default_tv_freq    = 61250000L;
 /* 104.3 MHz, a usable FM station for my area */
 static int default_radio_freq = 104300000L;
 
@@ -1987,6 +1987,34 @@ static unsigned int pvr2_copy_i2c_addr_list(
 }
 
 
+static void pvr2_hdw_cx25840_vbi_hack(struct pvr2_hdw *hdw)
+{
+        /*
+          Mike Isely <isely@pobox.com> 19-Nov-2006 - This bit of nuttiness
+          for cx25840 causes that module to correctly set up its video
+          scaling.  This is really a problem in the cx25840 module itself,
+          but we work around it here.  The problem has not been seen in
+          ivtv because there VBI is supported and set up.  We don't do VBI
+          here (at least not yet) and thus we never attempted to even set
+          it up.
+        */
+        struct v4l2_format fmt;
+        if (hdw->decoder_client_id != PVR2_CLIENT_ID_CX25840) {
+                /* We're not using a cx25840 so don't enable the hack */
+                return;
+        }
+
+        pvr2_trace(PVR2_TRACE_INIT,
+                   "Module ID %u:"
+                   " Executing cx25840 VBI hack",
+                   hdw->decoder_client_id);
+        memset(&fmt, 0, sizeof(fmt));
+        fmt.type = V4L2_BUF_TYPE_SLICED_VBI_CAPTURE;
+        v4l2_device_call_all(&hdw->v4l2_dev, hdw->decoder_client_id,
+                             video, s_fmt, &fmt);
+}
+
+
 static int pvr2_hdw_load_subdev(struct pvr2_hdw *hdw,
                                 const struct pvr2_device_client_desc *cd)
 {
@@ -2078,30 +2106,6 @@ static int pvr2_hdw_load_subdev(struct pvr2_hdw *hdw,
         /* client-specific setup... */
         switch (mid) {
         case PVR2_CLIENT_ID_CX25840:
-                hdw->decoder_client_id = mid;
-                {
-                        /*
-                          Mike Isely <isely@pobox.com> 19-Nov-2006 - This
-                          bit of nuttiness for cx25840 causes that module
-                          to correctly set up its video scaling.  This is
-                          really a problem in the cx25840 module itself,
-                          but we work around it here.  The problem has not
-                          been seen in ivtv because there VBI is supported
-                          and set up.  We don't do VBI here (at least not
-                          yet) and thus we never attempted to even set it
-                          up.
-                        */
-                        struct v4l2_format fmt;
-                        pvr2_trace(PVR2_TRACE_INIT,
-                                   "Module ID %u:"
-                                   " Executing cx25840 VBI hack",
-                                   mid);
-                        memset(&fmt, 0, sizeof(fmt));
-                        fmt.type = V4L2_BUF_TYPE_SLICED_VBI_CAPTURE;
-                        v4l2_device_call_all(&hdw->v4l2_dev, mid,
-                                             video, s_fmt, &fmt);
-                }
-                break;
         case PVR2_CLIENT_ID_SAA7115:
                 hdw->decoder_client_id = mid;
                 break;
@@ -2202,6 +2206,8 @@ static void pvr2_hdw_setup_low(struct pvr2_hdw *hdw)
                 cptr->info->set_value(cptr,~0,cptr->info->default_value);
         }
 
+        pvr2_hdw_cx25840_vbi_hack(hdw);
+
         /* Set up special default values for the television and radio
            frequencies here.  It's not really important what these defaults
            are, but I set them to something usable in the Chicago area just
@@ -2954,6 +2960,7 @@ static void pvr2_subdev_update(struct pvr2_hdw *hdw)
                         vs = hdw->std_mask_cur;
                         v4l2_device_call_all(&hdw->v4l2_dev, 0,
                                              core, s_std, vs);
+                        pvr2_hdw_cx25840_vbi_hack(hdw);
                 }
                 hdw->tuner_signal_stale = !0;
                 hdw->cropcap_stale = !0;
@@ -4076,6 +4083,7 @@ int pvr2_hdw_cmd_decoder_reset(struct pvr2_hdw *hdw)
         if (hdw->decoder_client_id) {
                 v4l2_device_call_all(&hdw->v4l2_dev, hdw->decoder_client_id,
                                      core, reset, 0);
+                pvr2_hdw_cx25840_vbi_hack(hdw);
                 return 0;
         }
         pvr2_trace(PVR2_TRACE_INIT,
diff --git a/drivers/media/video/pvrusb2/pvrusb2-video-v4l.c b/drivers/media/video/pvrusb2/pvrusb2-video-v4l.c
index d2fe7c8f2c3a..4c96cf48c796 100644
--- a/drivers/media/video/pvrusb2/pvrusb2-video-v4l.c
+++ b/drivers/media/video/pvrusb2/pvrusb2-video-v4l.c
@@ -54,6 +54,11 @@ static const int routing_scheme0[] = {
         [PVR2_CVAL_INPUT_SVIDEO] =  SAA7115_SVIDEO2,
 };
 
+static const struct routing_scheme routing_def0 = {
+        .def = routing_scheme0,
+        .cnt = ARRAY_SIZE(routing_scheme0),
+};
+
 static const int routing_scheme1[] = {
         [PVR2_CVAL_INPUT_TV] = SAA7115_COMPOSITE4,
         [PVR2_CVAL_INPUT_RADIO] = SAA7115_COMPOSITE5,
@@ -61,15 +66,14 @@ static const int routing_scheme1[] = {
         [PVR2_CVAL_INPUT_SVIDEO] =  SAA7115_SVIDEO2, /* or SVIDEO0, it seems */
 };
 
-static const struct routing_scheme routing_schemes[] = {
-        [PVR2_ROUTING_SCHEME_HAUPPAUGE] = {
-                .def = routing_scheme0,
-                .cnt = ARRAY_SIZE(routing_scheme0),
-        },
-        [PVR2_ROUTING_SCHEME_ONAIR] = {
-                .def = routing_scheme1,
-                .cnt = ARRAY_SIZE(routing_scheme1),
-        },
+static const struct routing_scheme routing_def1 = {
+        .def = routing_scheme1,
+        .cnt = ARRAY_SIZE(routing_scheme1),
+};
+
+static const struct routing_scheme *routing_schemes[] = {
+        [PVR2_ROUTING_SCHEME_HAUPPAUGE] = &routing_def0,
+        [PVR2_ROUTING_SCHEME_ONAIR] = &routing_def1,
 };
 
 void pvr2_saa7115_subdev_update(struct pvr2_hdw *hdw, struct v4l2_subdev *sd)
@@ -81,12 +85,12 @@ void pvr2_saa7115_subdev_update(struct pvr2_hdw *hdw, struct v4l2_subdev *sd)
 
                 pvr2_trace(PVR2_TRACE_CHIPS, "subdev v4l2 set_input(%d)",
                            hdw->input_val);
-                if ((sid < ARRAY_SIZE(routing_schemes)) &&
-                    ((sp = routing_schemes + sid) != NULL) &&
-                    (hdw->input_val >= 0) &&
-                    (hdw->input_val < sp->cnt)) {
-                        input = sp->def[hdw->input_val];
-                } else {
+
+                sp = (sid < ARRAY_SIZE(routing_schemes)) ?
+                        routing_schemes[sid] : NULL;
+                if ((sp == NULL) ||
+                    (hdw->input_val < 0) ||
+                    (hdw->input_val >= sp->cnt)) {
                         pvr2_trace(PVR2_TRACE_ERROR_LEGS,
                                    "*** WARNING *** subdev v4l2 set_input:"
                                    " Invalid routing scheme (%u)"
@@ -94,6 +98,7 @@ void pvr2_saa7115_subdev_update(struct pvr2_hdw *hdw, struct v4l2_subdev *sd)
                                    sid, hdw->input_val);
                         return;
                 }
+                input = sp->def[hdw->input_val];
                 sd->ops->video->s_routing(sd, input, 0, 0);
         }
 }
diff --git a/drivers/media/video/pxa_camera.c b/drivers/media/video/pxa_camera.c
index f60de40fd21f..46e0d8ad880f 100644
--- a/drivers/media/video/pxa_camera.c
+++ b/drivers/media/video/pxa_camera.c
@@ -163,13 +163,6 @@
                         CICR0_EOFM | CICR0_FOM)
 
 /*
- * YUV422P picture size should be a multiple of 16, so the heuristic aligns
- * height, width on 4 byte boundaries to reach the 16 multiple for the size.
- */
-#define YUV422P_X_Y_ALIGN 4
-#define YUV422P_SIZE_ALIGN YUV422P_X_Y_ALIGN * YUV422P_X_Y_ALIGN
-
-/*
  * Structures
  */
 enum pxa_camera_active_dma {
@@ -1398,28 +1391,15 @@ static int pxa_camera_try_fmt(struct soc_camera_device *icd,
                 return -EINVAL;
         }
 
-        /* limit to pxa hardware capabilities */
-        if (pix->height < 32)
-                pix->height = 32;
-        if (pix->height > 2048)
-                pix->height = 2048;
-        if (pix->width < 48)
-                pix->width = 48;
-        if (pix->width > 2048)
-                pix->width = 2048;
-        pix->width &= ~0x01;
-
         /*
-         * YUV422P planar format requires images size to be a 16 bytes
-         * multiple. If not, zeros will be inserted between Y and U planes, and
-         * U and V planes, and YUV422P standard would be violated.
+         * Limit to pxa hardware capabilities.  YUV422P planar format requires
+         * images size to be a multiple of 16 bytes.  If not, zeros will be
+         * inserted between Y and U planes, and U and V planes, which violates
+         * the YUV422P standard.
          */
-        if (xlate->host_fmt->fourcc == V4L2_PIX_FMT_YUV422P) {
-                if (!IS_ALIGNED(pix->width * pix->height, YUV422P_SIZE_ALIGN))
-                        pix->height = ALIGN(pix->height, YUV422P_X_Y_ALIGN);
-                if (!IS_ALIGNED(pix->width * pix->height, YUV422P_SIZE_ALIGN))
-                        pix->width = ALIGN(pix->width, YUV422P_X_Y_ALIGN);
-        }
+        v4l_bound_align_image(&pix->width, 48, 2048, 1,
+                              &pix->height, 32, 2048, 0,
+                              xlate->host_fmt->fourcc == V4L2_PIX_FMT_YUV422P ? 4 : 0);
 
         pix->bytesperline = pix->width *
                 DIV_ROUND_UP(xlate->host_fmt->depth, 8);
diff --git a/drivers/media/video/saa7134/saa7134-video.c b/drivers/media/video/saa7134/saa7134-video.c
index e305c1674cee..ba87128542e0 100644
--- a/drivers/media/video/saa7134/saa7134-video.c
+++ b/drivers/media/video/saa7134/saa7134-video.c
@@ -1640,15 +1640,8 @@ static int saa7134_try_fmt_vid_cap(struct file *file, void *priv,
         }
 
         f->fmt.pix.field = field;
-        if (f->fmt.pix.width  < 48)
-                f->fmt.pix.width  = 48;
-        if (f->fmt.pix.height < 32)
-                f->fmt.pix.height = 32;
-        if (f->fmt.pix.width > maxw)
-                f->fmt.pix.width = maxw;
-        if (f->fmt.pix.height > maxh)
-                f->fmt.pix.height = maxh;
-        f->fmt.pix.width &= ~0x03;
+        v4l_bound_align_image(&f->fmt.pix.width, 48, maxw, 2,
+                              &f->fmt.pix.height, 32, maxh, 0, 0);
         f->fmt.pix.bytesperline =
                 (f->fmt.pix.width * fmt->depth) >> 3;
         f->fmt.pix.sizeimage =
diff --git a/drivers/media/video/sh_mobile_ceu_camera.c b/drivers/media/video/sh_mobile_ceu_camera.c
index d369e8409ab8..0db88a53d92c 100644
--- a/drivers/media/video/sh_mobile_ceu_camera.c
+++ b/drivers/media/video/sh_mobile_ceu_camera.c
@@ -689,16 +689,8 @@ static int sh_mobile_ceu_try_fmt(struct soc_camera_device *icd,
 
         /* FIXME: calculate using depth and bus width */
 
-        if (f->fmt.pix.height < 4)
-                f->fmt.pix.height = 4;
-        if (f->fmt.pix.height > 1920)
-                f->fmt.pix.height = 1920;
-        if (f->fmt.pix.width < 2)
-                f->fmt.pix.width = 2;
-        if (f->fmt.pix.width > 2560)
-                f->fmt.pix.width = 2560;
-        f->fmt.pix.width &= ~0x01;
-        f->fmt.pix.height &= ~0x03;
+        v4l_bound_align_image(&f->fmt.pix.width, 2, 2560, 1,
+                              &f->fmt.pix.height, 4, 1920, 2, 0);
 
         f->fmt.pix.bytesperline = f->fmt.pix.width *
                 DIV_ROUND_UP(xlate->host_fmt->depth, 8);
diff --git a/drivers/media/video/tcm825x.c b/drivers/media/video/tcm825x.c
index b30c49248217..b90e9da3167d 100644
--- a/drivers/media/video/tcm825x.c
+++ b/drivers/media/video/tcm825x.c
@@ -878,7 +878,7 @@ static int tcm825x_probe(struct i2c_client *client,
         return rval;
 }
 
-static int __exit tcm825x_remove(struct i2c_client *client)
+static int tcm825x_remove(struct i2c_client *client)
 {
         struct tcm825x_sensor *sensor = i2c_get_clientdata(client);
 
@@ -902,7 +902,7 @@ static struct i2c_driver tcm825x_i2c_driver = {
                 .name = TCM825X_NAME,
         },
         .probe  = tcm825x_probe,
-        .remove = __exit_p(tcm825x_remove),
+        .remove = tcm825x_remove,
         .id_table = tcm825x_id,
 };
 
diff --git a/drivers/media/video/usbvideo/Kconfig b/drivers/media/video/usbvideo/Kconfig
index e4cb99c1f94b..adb1c044ad7d 100644
--- a/drivers/media/video/usbvideo/Kconfig
+++ b/drivers/media/video/usbvideo/Kconfig
@@ -38,10 +38,13 @@ config USB_KONICAWC
           module will be called konicawc.
 
 config USB_QUICKCAM_MESSENGER
-        tristate "USB Logitech Quickcam Messenger"
+        tristate "USB Logitech Quickcam Messenger (DEPRECATED)"
         depends on VIDEO_V4L1
         select VIDEO_USBVIDEO
         ---help---
+          This driver is DEPRECATED please use the gspca stv06xx module
+          instead.
+
           Say Y or M here to enable support for the USB Logitech Quickcam
           Messenger webcam.
 
diff --git a/drivers/media/video/v4l2-common.c b/drivers/media/video/v4l2-common.c
index f96475626da7..b91d66a767d7 100644
--- a/drivers/media/video/v4l2-common.c
+++ b/drivers/media/video/v4l2-common.c
@@ -802,6 +802,17 @@ struct v4l2_subdev *v4l2_i2c_new_subdev(struct v4l2_device *v4l2_dev,
         /* Decrease the module use count to match the first try_module_get. */
         module_put(client->driver->driver.owner);
 
+        if (sd) {
+                /* We return errors from v4l2_subdev_call only if we have the
+                   callback as the .s_config is not mandatory */
+                int err = v4l2_subdev_call(sd, core, s_config, 0, NULL);
+
+                if (err && err != -ENOIOCTLCMD) {
+                        v4l2_device_unregister_subdev(sd);
+                        sd = NULL;
+                }
+        }
+
 error:
         /* If we have a client but no subdev, then something went wrong and
            we must unregister the client. */
@@ -852,6 +863,17 @@ struct v4l2_subdev *v4l2_i2c_new_probed_subdev(struct v4l2_device *v4l2_dev,
         /* Decrease the module use count to match the first try_module_get. */
         module_put(client->driver->driver.owner);
 
+        if (sd) {
+                /* We return errors from v4l2_subdev_call only if we have the
+                   callback as the .s_config is not mandatory */
+                int err = v4l2_subdev_call(sd, core, s_config, 0, NULL);
+
+                if (err && err != -ENOIOCTLCMD) {
+                        v4l2_device_unregister_subdev(sd);
+                        sd = NULL;
+                }
+        }
+
 error:
         /* If we have a client but no subdev, then something went wrong and
            we must unregister the client. */
@@ -872,6 +894,89 @@ struct v4l2_subdev *v4l2_i2c_new_probed_subdev_addr(struct v4l2_device *v4l2_dev
 }
 EXPORT_SYMBOL_GPL(v4l2_i2c_new_probed_subdev_addr);
 
+/* Load an i2c sub-device. */
+struct v4l2_subdev *v4l2_i2c_new_subdev_board(struct v4l2_device *v4l2_dev,
+                struct i2c_adapter *adapter, const char *module_name,
+                struct i2c_board_info *info, const unsigned short *probe_addrs)
+{
+        struct v4l2_subdev *sd = NULL;
+        struct i2c_client *client;
+
+        BUG_ON(!v4l2_dev);
+
+        if (module_name)
+                request_module(module_name);
+
+        /* Create the i2c client */
+        if (info->addr == 0 && probe_addrs)
+                client = i2c_new_probed_device(adapter, info, probe_addrs);
+        else
+                client = i2c_new_device(adapter, info);
+
+        /* Note: by loading the module first we are certain that c->driver
+           will be set if the driver was found. If the module was not loaded
+           first, then the i2c core tries to delay-load the module for us,
+           and then c->driver is still NULL until the module is finally
+           loaded. This delay-load mechanism doesn't work if other drivers
+           want to use the i2c device, so explicitly loading the module
+           is the best alternative. */
+        if (client == NULL || client->driver == NULL)
+                goto error;
+
+        /* Lock the module so we can safely get the v4l2_subdev pointer */
+        if (!try_module_get(client->driver->driver.owner))
+                goto error;
+        sd = i2c_get_clientdata(client);
+
+        /* Register with the v4l2_device which increases the module's
+           use count as well. */
+        if (v4l2_device_register_subdev(v4l2_dev, sd))
+                sd = NULL;
+        /* Decrease the module use count to match the first try_module_get. */
+        module_put(client->driver->driver.owner);
+
+        if (sd) {
+                /* We return errors from v4l2_subdev_call only if we have the
+                   callback as the .s_config is not mandatory */
+                int err = v4l2_subdev_call(sd, core, s_config,
+                                info->irq, info->platform_data);
+
+                if (err && err != -ENOIOCTLCMD) {
+                        v4l2_device_unregister_subdev(sd);
+                        sd = NULL;
+                }
+        }
+
+error:
+        /* If we have a client but no subdev, then something went wrong and
+           we must unregister the client. */
+        if (client && sd == NULL)
+                i2c_unregister_device(client);
+        return sd;
+}
+EXPORT_SYMBOL_GPL(v4l2_i2c_new_subdev_board);
+
+struct v4l2_subdev *v4l2_i2c_new_subdev_cfg(struct v4l2_device *v4l2_dev,
+                struct i2c_adapter *adapter,
+                const char *module_name, const char *client_type,
+                int irq, void *platform_data,
+                u8 addr, const unsigned short *probe_addrs)
+{
+        struct i2c_board_info info;
+
+        /* Setup the i2c board info with the device type and
+           the device address. */
+        memset(&info, 0, sizeof(info));
+        strlcpy(info.type, client_type, sizeof(info.type));
+        info.addr = addr;
+        info.irq = irq;
+        info.platform_data = platform_data;
+
+        return v4l2_i2c_new_subdev_board(v4l2_dev, adapter, module_name,
+                        &info, probe_addrs);
+}
+EXPORT_SYMBOL_GPL(v4l2_i2c_new_subdev_cfg);
+
 /* Return i2c client address of v4l2_subdev. */
 unsigned short v4l2_i2c_subdev_addr(struct v4l2_subdev *sd)
 {
@@ -916,4 +1021,78 @@ const unsigned short *v4l2_i2c_tuner_addrs(enum v4l2_i2c_tuner_type type)
 }
 EXPORT_SYMBOL_GPL(v4l2_i2c_tuner_addrs);
 
-#endif
+#endif /* defined(CONFIG_I2C) */
+
+/* Clamp x to be between min and max, aligned to a multiple of 2^align.  min
+ * and max don't have to be aligned, but there must be at least one valid
+ * value.  E.g., min=17,max=31,align=4 is not allowed as there are no multiples
+ * of 16 between 17 and 31.  */
+static unsigned int clamp_align(unsigned int x, unsigned int min,
+                                unsigned int max, unsigned int align)
+{
+        /* Bits that must be zero to be aligned */
+        unsigned int mask = ~((1 << align) - 1);
+
+        /* Round to nearest aligned value */
+        if (align)
+                x = (x + (1 << (align - 1))) & mask;
+
+        /* Clamp to aligned value of min and max */
+        if (x < min)
+                x = (min + ~mask) & mask;
+        else if (x > max)
+                x = max & mask;
+
+        return x;
+}
+
+/* Bound an image to have a width between wmin and wmax, and height between
+ * hmin and hmax, inclusive.  Additionally, the width will be a multiple of
+ * 2^walign, the height will be a multiple of 2^halign, and the overall size
+ * (width*height) will be a multiple of 2^salign.  The image may be shrunk
+ * or enlarged to fit the alignment constraints.
+ *
+ * The width or height maximum must not be smaller than the corresponding
+ * minimum.  The alignments must not be so high there are no possible image
+ * sizes within the allowed bounds.  wmin and hmin must be at least 1
+ * (don't use 0).  If you don't care about a certain alignment, specify 0,
+ * as 2^0 is 1 and one byte alignment is equivalent to no alignment.  If
+ * you only want to adjust downward, specify a maximum that's the same as
+ * the initial value.
+ */
+void v4l_bound_align_image(u32 *w, unsigned int wmin, unsigned int wmax,
+                           unsigned int walign,
+                           u32 *h, unsigned int hmin, unsigned int hmax,
+                           unsigned int halign, unsigned int salign)
+{
+        *w = clamp_align(*w, wmin, wmax, walign);
+        *h = clamp_align(*h, hmin, hmax, halign);
+
+        /* Usually we don't need to align the size and are done now. */
+        if (!salign)
+                return;
+
+        /* How much alignment do we have? */
+        walign = __ffs(*w);
+        halign = __ffs(*h);
+        /* Enough to satisfy the image alignment? */
+        if (walign + halign < salign) {
+                /* Max walign where there is still a valid width */
+                unsigned int wmaxa = __fls(wmax ^ (wmin - 1));
+                /* Max halign where there is still a valid height */
+                unsigned int hmaxa = __fls(hmax ^ (hmin - 1));
+
+                /* up the smaller alignment until we have enough */
+                do {
+                        if (halign >= hmaxa ||
+                            (walign <= halign && walign < wmaxa)) {
+                                *w = clamp_align(*w, wmin, wmax, walign + 1);
+                                walign = __ffs(*w);
+                        } else {
+                                *h = clamp_align(*h, hmin, hmax, halign + 1);
+                                halign = __ffs(*h);
+                        }
+                } while (halign + walign < salign);
+        }
+}
+EXPORT_SYMBOL_GPL(v4l_bound_align_image);
diff --git a/drivers/media/video/vivi.c b/drivers/media/video/vivi.c
index fbfefae7886f..cd7266858462 100644
--- a/drivers/media/video/vivi.c
+++ b/drivers/media/video/vivi.c
@@ -883,15 +883,8 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
         maxh  = norm_maxh();
 
         f->fmt.pix.field = field;
-        if (f->fmt.pix.height < 32)
-                f->fmt.pix.height = 32;
-        if (f->fmt.pix.height > maxh)
-                f->fmt.pix.height = maxh;
-        if (f->fmt.pix.width < 48)
-                f->fmt.pix.width = 48;
-        if (f->fmt.pix.width > maxw)
-                f->fmt.pix.width = maxw;
-        f->fmt.pix.width &= ~0x03;
+        v4l_bound_align_image(&f->fmt.pix.width, 48, maxw, 2,
+                              &f->fmt.pix.height, 32, maxh, 0, 0);
         f->fmt.pix.bytesperline =
                 (f->fmt.pix.width * fmt->depth) >> 3;
         f->fmt.pix.sizeimage =
diff --git a/drivers/media/video/w9968cf.c b/drivers/media/video/w9968cf.c
index f59b2bd07e89..6c3f23e31b5c 100644
--- a/drivers/media/video/w9968cf.c
+++ b/drivers/media/video/w9968cf.c
@@ -460,7 +460,7 @@ static int w9968cf_set_picture(struct w9968cf_device*, struct video_picture);
 static int w9968cf_set_window(struct w9968cf_device*, struct video_window);
 static int w9968cf_postprocess_frame(struct w9968cf_device*,
                                      struct w9968cf_frame_t*);
-static int w9968cf_adjust_window_size(struct w9968cf_device*, u16* w, u16* h);
+static int w9968cf_adjust_window_size(struct w9968cf_device*, u32 *w, u32 *h);
 static void w9968cf_init_framelist(struct w9968cf_device*);
 static void w9968cf_push_frame(struct w9968cf_device*, u8 f_num);
 static void w9968cf_pop_frame(struct w9968cf_device*,struct w9968cf_frame_t**);
@@ -1763,8 +1763,7 @@ w9968cf_set_window(struct w9968cf_device* cam, struct video_window win)
         #define UNSC(x) ((x) >> 10)
 
         /* Make sure we are using a supported resolution */
-        if ((err = w9968cf_adjust_window_size(cam, (u16*)&win.width,
-                                              (u16*)&win.height)))
+        if ((err = w9968cf_adjust_window_size(cam, &win.width, &win.height)))
                 goto error;
 
         /* Scaling factors */
@@ -1914,12 +1913,9 @@ error:
   Return 0 on success, -1 otherwise.
   --------------------------------------------------------------------------*/
 static int
-w9968cf_adjust_window_size(struct w9968cf_device* cam, u16* width, u16* height)
+w9968cf_adjust_window_size(struct w9968cf_device *cam, u32 *width, u32 *height)
 {
-        u16 maxw, maxh;
-
-        if ((*width < cam->minwidth) || (*height < cam->minheight))
-                return -ERANGE;
+        unsigned int maxw, maxh, align;
 
         maxw = cam->upscaling && !(cam->vpp_flag & VPP_DECOMPRESSION) &&
                w9968cf_vpp ? max((u16)W9968CF_MAX_WIDTH, cam->maxwidth)
@@ -1927,16 +1923,10 @@ w9968cf_adjust_window_size(struct w9968cf_device* cam, u16* width, u16* height)
         maxh = cam->upscaling && !(cam->vpp_flag & VPP_DECOMPRESSION) &&
                w9968cf_vpp ? max((u16)W9968CF_MAX_HEIGHT, cam->maxheight)
                            : cam->maxheight;
+        align = (cam->vpp_flag & VPP_DECOMPRESSION) ? 4 : 0;
 
-        if (*width > maxw)
-                *width = maxw;
-        if (*height > maxh)
-                *height = maxh;
-
-        if (cam->vpp_flag & VPP_DECOMPRESSION) {
-                *width  &= ~15L; /* multiple of 16 */
-                *height &= ~15L;
-        }
+        v4l_bound_align_image(width, cam->minwidth, maxw, align,
+                              height, cam->minheight, maxh, align, 0);
 
         PDBGG("Window size adjusted w=%u, h=%u ", *width, *height)
 
@@ -3043,8 +3033,8 @@ static long w9968cf_v4l_ioctl(struct file *filp,
                 if (win.clipcount != 0 || win.flags != 0)
                         return -EINVAL;
 
-                if ((err = w9968cf_adjust_window_size(cam, (u16*)&win.width,
-                                                      (u16*)&win.height))) {
+                if ((err = w9968cf_adjust_window_size(cam, &win.width,
+                                                      &win.height))) {
                         DBG(4, "Resolution not supported (%ux%u). "
                                "VIDIOCSWIN failed", win.width, win.height)
                         return err;
@@ -3116,6 +3106,7 @@ static long w9968cf_v4l_ioctl(struct file *filp,
         {
                 struct video_mmap mmap;
                 struct w9968cf_frame_t* fr;
+                u32 w, h;
                 int err = 0;
 
                 if (copy_from_user(&mmap, arg, sizeof(mmap)))
@@ -3164,8 +3155,10 @@ static long w9968cf_v4l_ioctl(struct file *filp,
                    }
                 }
 
-                if ((err = w9968cf_adjust_window_size(cam, (u16*)&mmap.width,
-                                                      (u16*)&mmap.height))) {
+                w = mmap.width; h = mmap.height;
+                err = w9968cf_adjust_window_size(cam, &w, &h);
+                mmap.width = w; mmap.height = h;
+                if (err) {
                         DBG(4, "Resolution not supported (%dx%d). "
                                "VIDIOCMCAPTURE failed",
                             mmap.width, mmap.height)
diff --git a/drivers/media/video/zoran/zoran_driver.c b/drivers/media/video/zoran/zoran_driver.c
index 643cccaa1aab..3d7df32a3d87 100644
--- a/drivers/media/video/zoran/zoran_driver.c
+++ b/drivers/media/video/zoran/zoran_driver.c
@@ -2088,16 +2088,10 @@ static int zoran_try_fmt_vid_cap(struct file *file, void *__fh,
                 return -EINVAL;
         }
 
-        bpp = (zoran_formats[i].depth + 7) / 8;
-        fmt->fmt.pix.width &= ~((bpp == 2) ? 1 : 3);
-        if (fmt->fmt.pix.width > BUZ_MAX_WIDTH)
-                fmt->fmt.pix.width = BUZ_MAX_WIDTH;
-        if (fmt->fmt.pix.width < BUZ_MIN_WIDTH)
-                fmt->fmt.pix.width = BUZ_MIN_WIDTH;
-        if (fmt->fmt.pix.height > BUZ_MAX_HEIGHT)
-                fmt->fmt.pix.height = BUZ_MAX_HEIGHT;
-        if (fmt->fmt.pix.height < BUZ_MIN_HEIGHT)
-                fmt->fmt.pix.height = BUZ_MIN_HEIGHT;
+        bpp = DIV_ROUND_UP(zoran_formats[i].depth, 8);
+        v4l_bound_align_image(
+                &fmt->fmt.pix.width, BUZ_MIN_WIDTH, BUZ_MAX_WIDTH, bpp == 2 ? 1 : 2,
+                &fmt->fmt.pix.height, BUZ_MIN_HEIGHT, BUZ_MAX_HEIGHT, 0, 0);
         mutex_unlock(&zr->resource_lock);
 
         return 0;
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index 40111a6d8d5b..891ef18bd77b 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -94,6 +94,31 @@ config MMC_SDHCI_PLTFM
 
           If unsure, say N.
 
+config MMC_SDHCI_S3C
+        tristate "SDHCI support on Samsung S3C SoC"
+        depends on MMC_SDHCI && (PLAT_S3C24XX || PLAT_S3C64XX)
+        help
+          This selects the Secure Digital Host Controller Interface (SDHCI)
+          often referrered to as the HSMMC block in some of the Samsung S3C
+          range of SoC.
+
+          Note, due to the problems with DMA, the DMA support is only
+          available with CONFIG_EXPERIMENTAL is selected.
+
+          If you have a controller with this interface, say Y or M here.
+
+          If unsure, say N.
+
+config MMC_SDHCI_S3C_DMA
+        bool "DMA support on S3C SDHCI"
+        depends on MMC_SDHCI_S3C && EXPERIMENTAL
+        help
+          Enable DMA support on the Samsung S3C SDHCI glue. The DMA
+          has proved to be problematic if the controller encounters
+          certain errors, and thus should be treated with care.
+
+          YMMV.
+
 config MMC_OMAP
         tristate "TI OMAP Multimedia Card Interface support"
         depends on ARCH_OMAP
@@ -265,3 +290,14 @@ config MMC_CB710
           This driver can also be built as a module. If so, the module
           will be called cb710-mmc.
 
+config MMC_VIA_SDMMC
+        tristate "VIA SD/MMC Card Reader Driver"
+        depends on PCI
+        help
+          This selects the VIA SD/MMC Card Reader driver, say Y or M here.
+          VIA provides one multi-functional card reader which integrated into
+          some motherboards manufactured by VIA. This card reader supports
+          SD/MMC/SDHC.
+          If you have a controller with this interface, say Y or M here.
+
+          If unsure, say N.
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index 79da397c5fea..cf153f628457 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -15,6 +15,7 @@ obj-$(CONFIG_MMC_SDHCI_PCI)	+= sdhci-pci.o
 obj-$(CONFIG_MMC_RICOH_MMC)     += ricoh_mmc.o
 obj-$(CONFIG_MMC_SDHCI_OF)      += sdhci-of.o
 obj-$(CONFIG_MMC_SDHCI_PLTFM)   += sdhci-pltfm.o
+obj-$(CONFIG_MMC_SDHCI_S3C)     += sdhci-s3c.o
 obj-$(CONFIG_MMC_WBSD)          += wbsd.o
 obj-$(CONFIG_MMC_AU1X)          += au1xmmc.o
 obj-$(CONFIG_MMC_OMAP)          += omap.o
@@ -31,6 +32,7 @@ obj-$(CONFIG_MMC_S3C)   	+= s3cmci.o
 obj-$(CONFIG_MMC_SDRICOH_CS)    += sdricoh_cs.o
 obj-$(CONFIG_MMC_TMIO)          += tmio_mmc.o
 obj-$(CONFIG_MMC_CB710) += cb710-mmc.o
+obj-$(CONFIG_MMC_VIA_SDMMC)     += via-sdmmc.o
 
 ifeq ($(CONFIG_CB710_DEBUG),y)
         CFLAGS-cb710-mmc        += -DDEBUG
diff --git a/drivers/mmc/host/s3cmci.c b/drivers/mmc/host/s3cmci.c
index 4eb4f37544ab..8c08cd7efa7f 100644
--- a/drivers/mmc/host/s3cmci.c
+++ b/drivers/mmc/host/s3cmci.c
@@ -794,7 +794,7 @@ static void s3cmci_dma_setup(struct s3cmci_host *host,
                               host->mem->start + host->sdidata);
 
         if (!setup_ok) {
-                s3c2410_dma_config(host->dma, 4, 0);
+                s3c2410_dma_config(host->dma, 4);
                 s3c2410_dma_set_buffdone_fn(host->dma,
                                             s3cmci_dma_done_callback);
                 s3c2410_dma_setflags(host->dma, S3C2410_DMAF_AUTOSTART);
diff --git a/drivers/mmc/host/sdhci-of.c b/drivers/mmc/host/sdhci-of.c
index 128c614d11aa..d79fa55c3b89 100644
--- a/drivers/mmc/host/sdhci-of.c
+++ b/drivers/mmc/host/sdhci-of.c
@@ -250,6 +250,9 @@ static int __devinit sdhci_of_probe(struct of_device *ofdev,
                 host->ops = &sdhci_of_data->ops;
         }
 
+        if (of_get_property(np, "sdhci,1-bit-only", NULL))
+                host->quirks |= SDHCI_QUIRK_FORCE_1_BIT_DATA;
+
         clk = of_get_property(np, "clock-frequency", &size);
         if (clk && size == sizeof(*clk) && *clk)
                 of_host->clock = *clk;
diff --git a/drivers/mmc/host/sdhci-pci.c b/drivers/mmc/host/sdhci-pci.c
index 65be27995d5c..2f15cc17d887 100644
--- a/drivers/mmc/host/sdhci-pci.c
+++ b/drivers/mmc/host/sdhci-pci.c
@@ -284,6 +284,18 @@ static const struct sdhci_pci_fixes sdhci_jmicron = {
         .resume         = jmicron_resume,
 };
 
+static int via_probe(struct sdhci_pci_chip *chip)
+{
+        if (chip->pdev->revision == 0x10)
+                chip->quirks |= SDHCI_QUIRK_DELAY_AFTER_POWER;
+
+        return 0;
+}
+
+static const struct sdhci_pci_fixes sdhci_via = {
+        .probe          = via_probe,
+};
+
 static const struct pci_device_id pci_ids[] __devinitdata = {
         {
                 .vendor         = PCI_VENDOR_ID_RICOH,
@@ -349,6 +361,14 @@ static const struct pci_device_id pci_ids[] __devinitdata = {
                 .driver_data    = (kernel_ulong_t)&sdhci_jmicron,
         },
 
+        {
+                .vendor         = PCI_VENDOR_ID_VIA,
+                .device         = 0x95d0,
+                .subvendor      = PCI_ANY_ID,
+                .subdevice      = PCI_ANY_ID,
+                .driver_data    = (kernel_ulong_t)&sdhci_via,
+        },
+
         {       /* Generic SD host controller */
                 PCI_DEVICE_CLASS((PCI_CLASS_SYSTEM_SDHCI << 8), 0xFFFF00)
         },
diff --git a/drivers/mmc/host/sdhci-s3c.c b/drivers/mmc/host/sdhci-s3c.c
new file mode 100644
index 000000000000..50997d2a63e7
--- /dev/null
+++ b/drivers/mmc/host/sdhci-s3c.c
@@ -0,0 +1,428 @@
+/* linux/drivers/mmc/host/sdhci-s3c.c
+ *
+ * Copyright 2008 Openmoko Inc.
+ * Copyright 2008 Simtec Electronics
+ *      Ben Dooks <ben@simtec.co.uk>
+ *      http://armlinux.simtec.co.uk/
+ *
+ * SDHCI (HSMMC) support for Samsung SoC
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+
+#include <linux/mmc/host.h>
+
+#include <plat/sdhci.h>
+#include <plat/regs-sdhci.h>
+
+#include "sdhci.h"
+
+#define MAX_BUS_CLK     (4)
+
+/**
+ * struct sdhci_s3c - S3C SDHCI instance
+ * @host: The SDHCI host created
+ * @pdev: The platform device we where created from.
+ * @ioarea: The resource created when we claimed the IO area.
+ * @pdata: The platform data for this controller.
+ * @cur_clk: The index of the current bus clock.
+ * @clk_io: The clock for the internal bus interface.
+ * @clk_bus: The clocks that are available for the SD/MMC bus clock.
+ */
+struct sdhci_s3c {
+        struct sdhci_host       *host;
+        struct platform_device  *pdev;
+        struct resource         *ioarea;
+        struct s3c_sdhci_platdata *pdata;
+        unsigned int            cur_clk;
+
+        struct clk              *clk_io;
+        struct clk              *clk_bus[MAX_BUS_CLK];
+};
+
+static inline struct sdhci_s3c *to_s3c(struct sdhci_host *host)
+{
+        return sdhci_priv(host);
+}
+
+/**
+ * get_curclk - convert ctrl2 register to clock source number
+ * @ctrl2: Control2 register value.
+ */
+static u32 get_curclk(u32 ctrl2)
+{
+        ctrl2 &= S3C_SDHCI_CTRL2_SELBASECLK_MASK;
+        ctrl2 >>= S3C_SDHCI_CTRL2_SELBASECLK_SHIFT;
+
+        return ctrl2;
+}
+
+static void sdhci_s3c_check_sclk(struct sdhci_host *host)
+{
+        struct sdhci_s3c *ourhost = to_s3c(host);
+        u32 tmp = readl(host->ioaddr + S3C_SDHCI_CONTROL2);
+
+        if (get_curclk(tmp) != ourhost->cur_clk) {
+                dev_dbg(&ourhost->pdev->dev, "restored ctrl2 clock setting\n");
+
+                tmp &= ~S3C_SDHCI_CTRL2_SELBASECLK_MASK;
+                tmp |= ourhost->cur_clk << S3C_SDHCI_CTRL2_SELBASECLK_SHIFT;
+                writel(tmp, host->ioaddr + 0x80);
+        }
+}
+
+/**
+ * sdhci_s3c_get_max_clk - callback to get maximum clock frequency.
+ * @host: The SDHCI host instance.
+ *
+ * Callback to return the maximum clock rate acheivable by the controller.
+*/
+static unsigned int sdhci_s3c_get_max_clk(struct sdhci_host *host)
+{
+        struct sdhci_s3c *ourhost = to_s3c(host);
+        struct clk *busclk;
+        unsigned int rate, max;
+        int clk;
+
+        /* note, a reset will reset the clock source */
+
+        sdhci_s3c_check_sclk(host);
+
+        for (max = 0, clk = 0; clk < MAX_BUS_CLK; clk++) {
+                busclk = ourhost->clk_bus[clk];
+                if (!busclk)
+                        continue;
+
+                rate = clk_get_rate(busclk);
+                if (rate > max)
+                        max = rate;
+        }
+
+        return max;
+}
+
+static unsigned int sdhci_s3c_get_timeout_clk(struct sdhci_host *host)
+{
+        return sdhci_s3c_get_max_clk(host) / 1000000;
+}
+
+/**
+ * sdhci_s3c_consider_clock - consider one the bus clocks for current setting
+ * @ourhost: Our SDHCI instance.
+ * @src: The source clock index.
+ * @wanted: The clock frequency wanted.
+ */
+static unsigned int sdhci_s3c_consider_clock(struct sdhci_s3c *ourhost,
+                                             unsigned int src,
+                                             unsigned int wanted)
+{
+        unsigned long rate;
+        struct clk *clksrc = ourhost->clk_bus[src];
+        int div;
+
+        if (!clksrc)
+                return UINT_MAX;
+
+        rate = clk_get_rate(clksrc);
+
+        for (div = 1; div < 256; div *= 2) {
+                if ((rate / div) <= wanted)
+                        break;
+        }
+
+        dev_dbg(&ourhost->pdev->dev, "clk %d: rate %ld, want %d, got %ld\n",
+                src, rate, wanted, rate / div);
+
+        return (wanted - (rate / div));
+}
+
+/**
+ * sdhci_s3c_set_clock - callback on clock change
+ * @host: The SDHCI host being changed
+ * @clock: The clock rate being requested.
+ *
+ * When the card's clock is going to be changed, look at the new frequency
+ * and find the best clock source to go with it.
+*/
+static void sdhci_s3c_set_clock(struct sdhci_host *host, unsigned int clock)
+{
+        struct sdhci_s3c *ourhost = to_s3c(host);
+        unsigned int best = UINT_MAX;
+        unsigned int delta;
+        int best_src = 0;
+        int src;
+        u32 ctrl;
+
+        /* don't bother if the clock is going off. */
+        if (clock == 0)
+                return;
+
+        for (src = 0; src < MAX_BUS_CLK; src++) {
+                delta = sdhci_s3c_consider_clock(ourhost, src, clock);
+                if (delta < best) {
+                        best = delta;
+                        best_src = src;
+                }
+        }
+
+        dev_dbg(&ourhost->pdev->dev,
+                "selected source %d, clock %d, delta %d\n",
+                 best_src, clock, best);
+
+        /* select the new clock source */
+
+        if (ourhost->cur_clk != best_src) {
+                struct clk *clk = ourhost->clk_bus[best_src];
+
+                /* turn clock off to card before changing clock source */
+                writew(0, host->ioaddr + SDHCI_CLOCK_CONTROL);
+
+                ourhost->cur_clk = best_src;
+                host->max_clk = clk_get_rate(clk);
+                host->timeout_clk = sdhci_s3c_get_timeout_clk(host);
+
+                ctrl = readl(host->ioaddr + S3C_SDHCI_CONTROL2);
+                ctrl &= ~S3C_SDHCI_CTRL2_SELBASECLK_MASK;
+                ctrl |= best_src << S3C_SDHCI_CTRL2_SELBASECLK_SHIFT;
+                writel(ctrl, host->ioaddr + S3C_SDHCI_CONTROL2);
+        }
+
+        /* reconfigure the hardware for new clock rate */
+
+        {
+                struct mmc_ios ios;
+
+                ios.clock = clock;
+
+                if (ourhost->pdata->cfg_card)
+                        (ourhost->pdata->cfg_card)(ourhost->pdev, host->ioaddr,
+                                                   &ios, NULL);
+        }
+}
+
+static struct sdhci_ops sdhci_s3c_ops = {
+        .get_max_clock          = sdhci_s3c_get_max_clk,
+        .get_timeout_clock      = sdhci_s3c_get_timeout_clk,
+        .set_clock              = sdhci_s3c_set_clock,
+};
+
+static int __devinit sdhci_s3c_probe(struct platform_device *pdev)
+{
+        struct s3c_sdhci_platdata *pdata = pdev->dev.platform_data;
+        struct device *dev = &pdev->dev;
+        struct sdhci_host *host;
+        struct sdhci_s3c *sc;
+        struct resource *res;
+        int ret, irq, ptr, clks;
+
+        if (!pdata) {
+                dev_err(dev, "no device data specified\n");
+                return -ENOENT;
+        }
+
+        irq = platform_get_irq(pdev, 0);
+        if (irq < 0) {
+                dev_err(dev, "no irq specified\n");
+                return irq;
+        }
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res) {
+                dev_err(dev, "no memory specified\n");
+                return -ENOENT;
+        }
+
+        host = sdhci_alloc_host(dev, sizeof(struct sdhci_s3c));
+        if (IS_ERR(host)) {
+                dev_err(dev, "sdhci_alloc_host() failed\n");
+                return PTR_ERR(host);
+        }
+
+        sc = sdhci_priv(host);
+
+        sc->host = host;
+        sc->pdev = pdev;
+        sc->pdata = pdata;
+
+        platform_set_drvdata(pdev, host);
+
+        sc->clk_io = clk_get(dev, "hsmmc");
+        if (IS_ERR(sc->clk_io)) {
+                dev_err(dev, "failed to get io clock\n");
+                ret = PTR_ERR(sc->clk_io);
+                goto err_io_clk;
+        }
+
+        /* enable the local io clock and keep it running for the moment. */
+        clk_enable(sc->clk_io);
+
+        for (clks = 0, ptr = 0; ptr < MAX_BUS_CLK; ptr++) {
+                struct clk *clk;
+                char *name = pdata->clocks[ptr];
+
+                if (name == NULL)
+                        continue;
+
+                clk = clk_get(dev, name);
+                if (IS_ERR(clk)) {
+                        dev_err(dev, "failed to get clock %s\n", name);
+                        continue;
+                }
+
+                clks++;
+                sc->clk_bus[ptr] = clk;
+                clk_enable(clk);
+
+                dev_info(dev, "clock source %d: %s (%ld Hz)\n",
+                         ptr, name, clk_get_rate(clk));
+        }
+
+        if (clks == 0) {
+                dev_err(dev, "failed to find any bus clocks\n");
+                ret = -ENOENT;
+                goto err_no_busclks;
+        }
+
+        sc->ioarea = request_mem_region(res->start, resource_size(res),
+                                        mmc_hostname(host->mmc));
+        if (!sc->ioarea) {
+                dev_err(dev, "failed to reserve register area\n");
+                ret = -ENXIO;
+                goto err_req_regs;
+        }
+
+        host->ioaddr = ioremap_nocache(res->start, resource_size(res));
+        if (!host->ioaddr) {
+                dev_err(dev, "failed to map registers\n");
+                ret = -ENXIO;
+                goto err_req_regs;
+        }
+
+        /* Ensure we have minimal gpio selected CMD/CLK/Detect */
+        if (pdata->cfg_gpio)
+                pdata->cfg_gpio(pdev, pdata->max_width);
+
+        host->hw_name = "samsung-hsmmc";
+        host->ops = &sdhci_s3c_ops;
+        host->quirks = 0;
+        host->irq = irq;
+
+        /* Setup quirks for the controller */
+
+        /* Currently with ADMA enabled we are getting some length
+         * interrupts that are not being dealt with, do disable
+         * ADMA until this is sorted out. */
+        host->quirks |= SDHCI_QUIRK_BROKEN_ADMA;
+        host->quirks |= SDHCI_QUIRK_32BIT_ADMA_SIZE;
+
+#ifndef CONFIG_MMC_SDHCI_S3C_DMA
+
+        /* we currently see overruns on errors, so disable the SDMA
+         * support as well. */
+        host->quirks |= SDHCI_QUIRK_BROKEN_DMA;
+
+        /* PIO currently has problems with multi-block IO */
+        host->quirks |= SDHCI_QUIRK_NO_MULTIBLOCK;
+
+#endif /* CONFIG_MMC_SDHCI_S3C_DMA */
+
+        /* It seems we do not get an DATA transfer complete on non-busy
+         * transfers, not sure if this is a problem with this specific
+         * SDHCI block, or a missing configuration that needs to be set. */
+        host->quirks |= SDHCI_QUIRK_NO_BUSY_IRQ;
+
+        host->quirks |= (SDHCI_QUIRK_32BIT_DMA_ADDR |
+                         SDHCI_QUIRK_32BIT_DMA_SIZE);
+
+        ret = sdhci_add_host(host);
+        if (ret) {
+                dev_err(dev, "sdhci_add_host() failed\n");
+                goto err_add_host;
+        }
+
+        return 0;
+
+ err_add_host:
+        release_resource(sc->ioarea);
+        kfree(sc->ioarea);
+
+ err_req_regs:
+        for (ptr = 0; ptr < MAX_BUS_CLK; ptr++) {
+                clk_disable(sc->clk_bus[ptr]);
+                clk_put(sc->clk_bus[ptr]);
+        }
+
+ err_no_busclks:
+        clk_disable(sc->clk_io);
+        clk_put(sc->clk_io);
+
+ err_io_clk:
+        sdhci_free_host(host);
+
+        return ret;
+}
+
+static int __devexit sdhci_s3c_remove(struct platform_device *pdev)
+{
+        return 0;
+}
+
+#ifdef CONFIG_PM
+
+static int sdhci_s3c_suspend(struct platform_device *dev, pm_message_t pm)
+{
+        struct sdhci_host *host = platform_get_drvdata(dev);
+
+        sdhci_suspend_host(host, pm);
+        return 0;
+}
+
+static int sdhci_s3c_resume(struct platform_device *dev)
+{
+        struct sdhci_host *host = platform_get_drvdata(dev);
+
+        sdhci_resume_host(host);
+        return 0;
+}
+
+#else
+#define sdhci_s3c_suspend NULL
+#define sdhci_s3c_resume NULL
+#endif
+
+static struct platform_driver sdhci_s3c_driver = {
+        .probe          = sdhci_s3c_probe,
+        .remove         = __devexit_p(sdhci_s3c_remove),
+        .suspend        = sdhci_s3c_suspend,
+        .resume         = sdhci_s3c_resume,
+        .driver         = {
+                .owner  = THIS_MODULE,
+                .name   = "s3c-sdhci",
+        },
+};
+
+static int __init sdhci_s3c_init(void)
+{
+        return platform_driver_register(&sdhci_s3c_driver);
+}
+
+static void __exit sdhci_s3c_exit(void)
+{
+        platform_driver_unregister(&sdhci_s3c_driver);
+}
+
+module_init(sdhci_s3c_init);
+module_exit(sdhci_s3c_exit);
+
+MODULE_DESCRIPTION("Samsung SDHCI (HSMMC) glue");
+MODULE_AUTHOR("Ben Dooks, <ben@simtec.co.uk>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:s3c-sdhci");
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 35789c6edc19..6779b4ecab18 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -584,7 +584,7 @@ static u8 sdhci_calc_timeout(struct sdhci_host *host, struct mmc_data *data)
          * longer to time out, but that's much better than having a too-short
          * timeout value.
          */
-        if ((host->quirks & SDHCI_QUIRK_BROKEN_TIMEOUT_VAL))
+        if (host->quirks & SDHCI_QUIRK_BROKEN_TIMEOUT_VAL)
                 return 0xE;
 
         /* timeout in us */
@@ -1051,12 +1051,19 @@ static void sdhci_set_power(struct sdhci_host *host, unsigned short power)
          * At least the Marvell CaFe chip gets confused if we set the voltage
          * and set turn on power at the same time, so set the voltage first.
          */
-        if ((host->quirks & SDHCI_QUIRK_NO_SIMULT_VDD_AND_POWER))
+        if (host->quirks & SDHCI_QUIRK_NO_SIMULT_VDD_AND_POWER)
                 sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
 
         pwr |= SDHCI_POWER_ON;
 
         sdhci_writeb(host, pwr, SDHCI_POWER_CONTROL);
+
+        /*
+         * Some controllers need an extra 10ms delay of 10ms before they
+         * can apply clock after applying power
+         */
+        if (host->quirks & SDHCI_QUIRK_DELAY_AFTER_POWER)
+                mdelay(10);
 }
 
 /*****************************************************************************\
@@ -1382,6 +1389,35 @@ static void sdhci_cmd_irq(struct sdhci_host *host, u32 intmask)
                 sdhci_finish_command(host);
 }
 
+#ifdef DEBUG
+static void sdhci_show_adma_error(struct sdhci_host *host)
+{
+        const char *name = mmc_hostname(host->mmc);
+        u8 *desc = host->adma_desc;
+        __le32 *dma;
+        __le16 *len;
+        u8 attr;
+
+        sdhci_dumpregs(host);
+
+        while (true) {
+                dma = (__le32 *)(desc + 4);
+                len = (__le16 *)(desc + 2);
+                attr = *desc;
+
+                DBG("%s: %p: DMA 0x%08x, LEN 0x%04x, Attr=0x%02x\n",
+                    name, desc, le32_to_cpu(*dma), le16_to_cpu(*len), attr);
+
+                desc += 8;
+
+                if (attr & 2)
+                        break;
+        }
+}
+#else
+static void sdhci_show_adma_error(struct sdhci_host *host) { }
+#endif
+
 static void sdhci_data_irq(struct sdhci_host *host, u32 intmask)
 {
         BUG_ON(intmask == 0);
@@ -1411,8 +1447,11 @@ static void sdhci_data_irq(struct sdhci_host *host, u32 intmask)
                 host->data->error = -ETIMEDOUT;
         else if (intmask & (SDHCI_INT_DATA_CRC | SDHCI_INT_DATA_END_BIT))
                 host->data->error = -EILSEQ;
-        else if (intmask & SDHCI_INT_ADMA_ERROR)
+        else if (intmask & SDHCI_INT_ADMA_ERROR) {
+                printk(KERN_ERR "%s: ADMA error\n", mmc_hostname(host->mmc));
+                sdhci_show_adma_error(host);
                 host->data->error = -EIO;
+        }
 
         if (host->data->error)
                 sdhci_finish_data(host);
@@ -1729,7 +1768,10 @@ int sdhci_add_host(struct sdhci_host *host)
         mmc->ops = &sdhci_ops;
         mmc->f_min = host->max_clk / 256;
         mmc->f_max = host->max_clk;
-        mmc->caps = MMC_CAP_4_BIT_DATA | MMC_CAP_SDIO_IRQ;
+        mmc->caps = MMC_CAP_SDIO_IRQ;
+
+        if (!(host->quirks & SDHCI_QUIRK_FORCE_1_BIT_DATA))
+                mmc->caps |= MMC_CAP_4_BIT_DATA;
 
         if (caps & SDHCI_CAN_DO_HISPD)
                 mmc->caps |= MMC_CAP_SD_HIGHSPEED;
@@ -1802,7 +1844,7 @@ int sdhci_add_host(struct sdhci_host *host)
         /*
          * Maximum block count.
          */
-        mmc->max_blk_count = 65535;
+        mmc->max_blk_count = (host->quirks & SDHCI_QUIRK_NO_MULTIBLOCK) ? 1 : 65535;
 
         /*
          * Init tasklets.
diff --git a/drivers/mmc/host/sdhci.h b/drivers/mmc/host/sdhci.h
index 2de08349c3ca..831ddf7dcb49 100644
--- a/drivers/mmc/host/sdhci.h
+++ b/drivers/mmc/host/sdhci.h
@@ -226,6 +226,12 @@ struct sdhci_host {
 #define SDHCI_QUIRK_RESTORE_IRQS_AFTER_RESET            (1<<19)
 /* Controller has to be forced to use block size of 2048 bytes */
 #define SDHCI_QUIRK_FORCE_BLK_SZ_2048                   (1<<20)
+/* Controller cannot do multi-block transfers */
+#define SDHCI_QUIRK_NO_MULTIBLOCK                       (1<<21)
+/* Controller can only handle 1-bit data transfers */
+#define SDHCI_QUIRK_FORCE_1_BIT_DATA                    (1<<22)
+/* Controller needs 10ms delay between applying power and clock */
+#define SDHCI_QUIRK_DELAY_AFTER_POWER                   (1<<23)
 
         int                     irq;            /* Device IRQ */
         void __iomem *          ioaddr;         /* Mapped address */
diff --git a/drivers/mmc/host/via-sdmmc.c b/drivers/mmc/host/via-sdmmc.c
new file mode 100644
index 000000000000..632858a94376
--- /dev/null
+++ b/drivers/mmc/host/via-sdmmc.c
@@ -0,0 +1,1362 @@
+/*
+ *  drivers/mmc/host/via-sdmmc.c - VIA SD/MMC Card Reader driver
+ *  Copyright (c) 2008, VIA Technologies Inc. All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or (at
+ * your option) any later version.
+ */
+
+#include <linux/pci.h>
+#include <linux/dma-mapping.h>
+#include <linux/highmem.h>
+#include <linux/delay.h>
+
+#include <linux/mmc/host.h>
+
+#define DRV_NAME        "via_sdmmc"
+
+#define PCI_DEVICE_ID_VIA_9530  0x9530
+
+#define VIA_CRDR_SDC_OFF        0x200
+#define VIA_CRDR_DDMA_OFF       0x400
+#define VIA_CRDR_PCICTRL_OFF    0x600
+
+#define VIA_CRDR_MIN_CLOCK      375000
+#define VIA_CRDR_MAX_CLOCK      48000000
+
+/*
+ * PCI registers
+ */
+
+#define VIA_CRDR_PCI_WORK_MODE  0x40
+#define VIA_CRDR_PCI_DBG_MODE   0x41
+
+/*
+ * SDC MMIO Registers
+ */
+
+#define VIA_CRDR_SDCTRL                 0x0
+#define VIA_CRDR_SDCTRL_START           0x01
+#define VIA_CRDR_SDCTRL_WRITE           0x04
+#define VIA_CRDR_SDCTRL_SINGLE_WR       0x10
+#define VIA_CRDR_SDCTRL_SINGLE_RD       0x20
+#define VIA_CRDR_SDCTRL_MULTI_WR        0x30
+#define VIA_CRDR_SDCTRL_MULTI_RD        0x40
+#define VIA_CRDR_SDCTRL_STOP            0x70
+
+#define VIA_CRDR_SDCTRL_RSP_NONE        0x0
+#define VIA_CRDR_SDCTRL_RSP_R1          0x10000
+#define VIA_CRDR_SDCTRL_RSP_R2          0x20000
+#define VIA_CRDR_SDCTRL_RSP_R3          0x30000
+#define VIA_CRDR_SDCTRL_RSP_R1B         0x90000
+
+#define VIA_CRDR_SDCARG         0x4
+
+#define VIA_CRDR_SDBUSMODE      0x8
+#define VIA_CRDR_SDMODE_4BIT    0x02
+#define VIA_CRDR_SDMODE_CLK_ON  0x40
+
+#define VIA_CRDR_SDBLKLEN       0xc
+/*
+ * Bit 0 -Bit 10 : Block length. So, the maximum block length should be 2048.
+ * Bit 11 - Bit 13 : Reserved.
+ * GPIDET : Select GPI pin to detect card, GPI means CR_CD# in top design.
+ * INTEN : Enable SD host interrupt.
+ * Bit 16 - Bit 31 : Block count. So, the maximun block count should be 65536.
+ */
+#define VIA_CRDR_SDBLKLEN_GPIDET        0x2000
+#define VIA_CRDR_SDBLKLEN_INTEN         0x8000
+#define VIA_CRDR_MAX_BLOCK_COUNT        65536
+#define VIA_CRDR_MAX_BLOCK_LENGTH       2048
+
+#define VIA_CRDR_SDRESP0        0x10
+#define VIA_CRDR_SDRESP1        0x14
+#define VIA_CRDR_SDRESP2        0x18
+#define VIA_CRDR_SDRESP3        0x1c
+
+#define VIA_CRDR_SDCURBLKCNT    0x20
+
+#define VIA_CRDR_SDINTMASK      0x24
+/*
+ * MBDIE : Multiple Blocks transfer Done Interrupt Enable
+ * BDDIE : Block Data transfer Done Interrupt Enable
+ * CIRIE : Card Insertion or Removal Interrupt Enable
+ * CRDIE : Command-Response transfer Done Interrupt Enable
+ * CRTOIE : Command-Response response TimeOut Interrupt Enable
+ * ASCRDIE : Auto Stop Command-Response transfer Done Interrupt Enable
+ * DTIE : Data access Timeout Interrupt Enable
+ * SCIE : reSponse CRC error Interrupt Enable
+ * RCIE : Read data CRC error Interrupt Enable
+ * WCIE : Write data CRC error Interrupt Enable
+ */
+#define VIA_CRDR_SDINTMASK_MBDIE        0x10
+#define VIA_CRDR_SDINTMASK_BDDIE        0x20
+#define VIA_CRDR_SDINTMASK_CIRIE        0x80
+#define VIA_CRDR_SDINTMASK_CRDIE        0x200
+#define VIA_CRDR_SDINTMASK_CRTOIE       0x400
+#define VIA_CRDR_SDINTMASK_ASCRDIE      0x800
+#define VIA_CRDR_SDINTMASK_DTIE         0x1000
+#define VIA_CRDR_SDINTMASK_SCIE         0x2000
+#define VIA_CRDR_SDINTMASK_RCIE         0x4000
+#define VIA_CRDR_SDINTMASK_WCIE         0x8000
+
+#define VIA_CRDR_SDACTIVE_INTMASK \
+        (VIA_CRDR_SDINTMASK_MBDIE | VIA_CRDR_SDINTMASK_CIRIE \
+        | VIA_CRDR_SDINTMASK_CRDIE | VIA_CRDR_SDINTMASK_CRTOIE \
+        | VIA_CRDR_SDINTMASK_DTIE | VIA_CRDR_SDINTMASK_SCIE \
+        | VIA_CRDR_SDINTMASK_RCIE | VIA_CRDR_SDINTMASK_WCIE)
+
+#define VIA_CRDR_SDSTATUS       0x28
+/*
+ * CECC : Reserved
+ * WP : SD card Write Protect status
+ * SLOTD : Reserved
+ * SLOTG : SD SLOT status(Gpi pin status)
+ * MBD : Multiple Blocks transfer Done interrupt status
+ * BDD : Block Data transfer Done interrupt status
+ * CD : Reserved
+ * CIR : Card Insertion or Removal interrupt detected on GPI pin
+ * IO : Reserved
+ * CRD : Command-Response transfer Done interrupt status
+ * CRTO : Command-Response response TimeOut interrupt status
+ * ASCRDIE : Auto Stop Command-Response transfer Done interrupt status
+ * DT : Data access Timeout interrupt status
+ * SC : reSponse CRC error interrupt status
+ * RC : Read data CRC error interrupt status
+ * WC : Write data CRC error interrupt status
+ */
+#define VIA_CRDR_SDSTS_CECC             0x01
+#define VIA_CRDR_SDSTS_WP               0x02
+#define VIA_CRDR_SDSTS_SLOTD            0x04
+#define VIA_CRDR_SDSTS_SLOTG            0x08
+#define VIA_CRDR_SDSTS_MBD              0x10
+#define VIA_CRDR_SDSTS_BDD              0x20
+#define VIA_CRDR_SDSTS_CD               0x40
+#define VIA_CRDR_SDSTS_CIR              0x80
+#define VIA_CRDR_SDSTS_IO               0x100
+#define VIA_CRDR_SDSTS_CRD              0x200
+#define VIA_CRDR_SDSTS_CRTO             0x400
+#define VIA_CRDR_SDSTS_ASCRDIE          0x800
+#define VIA_CRDR_SDSTS_DT               0x1000
+#define VIA_CRDR_SDSTS_SC               0x2000
+#define VIA_CRDR_SDSTS_RC               0x4000
+#define VIA_CRDR_SDSTS_WC               0x8000
+
+#define VIA_CRDR_SDSTS_IGN_MASK\
+        (VIA_CRDR_SDSTS_BDD | VIA_CRDR_SDSTS_ASCRDIE | VIA_CRDR_SDSTS_IO)
+#define VIA_CRDR_SDSTS_INT_MASK \
+        (VIA_CRDR_SDSTS_MBD | VIA_CRDR_SDSTS_BDD | VIA_CRDR_SDSTS_CD \
+        | VIA_CRDR_SDSTS_CIR | VIA_CRDR_SDSTS_IO | VIA_CRDR_SDSTS_CRD \
+        | VIA_CRDR_SDSTS_CRTO | VIA_CRDR_SDSTS_ASCRDIE | VIA_CRDR_SDSTS_DT \
+        | VIA_CRDR_SDSTS_SC | VIA_CRDR_SDSTS_RC | VIA_CRDR_SDSTS_WC)
+#define VIA_CRDR_SDSTS_W1C_MASK \
+        (VIA_CRDR_SDSTS_CECC | VIA_CRDR_SDSTS_MBD | VIA_CRDR_SDSTS_BDD \
+        | VIA_CRDR_SDSTS_CD | VIA_CRDR_SDSTS_CIR | VIA_CRDR_SDSTS_CRD \
+        | VIA_CRDR_SDSTS_CRTO | VIA_CRDR_SDSTS_ASCRDIE | VIA_CRDR_SDSTS_DT \
+        | VIA_CRDR_SDSTS_SC | VIA_CRDR_SDSTS_RC | VIA_CRDR_SDSTS_WC)
+#define  VIA_CRDR_SDSTS_CMD_MASK \
+        (VIA_CRDR_SDSTS_CRD | VIA_CRDR_SDSTS_CRTO | VIA_CRDR_SDSTS_SC)
+#define  VIA_CRDR_SDSTS_DATA_MASK\
+        (VIA_CRDR_SDSTS_MBD | VIA_CRDR_SDSTS_DT \
+        | VIA_CRDR_SDSTS_RC | VIA_CRDR_SDSTS_WC)
+
+#define VIA_CRDR_SDSTATUS2      0x2a
+/*
+ * CFE : Enable SD host automatic Clock FReezing
+ */
+#define VIA_CRDR_SDSTS_CFE              0x80
+
+#define VIA_CRDR_SDRSPTMO       0x2C
+
+#define VIA_CRDR_SDCLKSEL       0x30
+
+#define VIA_CRDR_SDEXTCTRL      0x34
+#define VIS_CRDR_SDEXTCTRL_AUTOSTOP_SD  0x01
+#define VIS_CRDR_SDEXTCTRL_SHIFT_9      0x02
+#define VIS_CRDR_SDEXTCTRL_MMC_8BIT     0x04
+#define VIS_CRDR_SDEXTCTRL_RELD_BLK     0x08
+#define VIS_CRDR_SDEXTCTRL_BAD_CMDA     0x10
+#define VIS_CRDR_SDEXTCTRL_BAD_DATA     0x20
+#define VIS_CRDR_SDEXTCTRL_AUTOSTOP_SPI 0x40
+#define VIA_CRDR_SDEXTCTRL_HISPD        0x80
+/* 0x38-0xFF reserved */
+
+/*
+ * Data DMA Control Registers
+ */
+
+#define VIA_CRDR_DMABASEADD     0x0
+#define VIA_CRDR_DMACOUNTER     0x4
+
+#define VIA_CRDR_DMACTRL        0x8
+/*
+ * DIR :Transaction Direction
+ * 0 : From card to memory
+ * 1 : From memory to card
+ */
+#define VIA_CRDR_DMACTRL_DIR            0x100
+#define VIA_CRDR_DMACTRL_ENIRQ          0x10000
+#define VIA_CRDR_DMACTRL_SFTRST         0x1000000
+
+#define VIA_CRDR_DMASTS         0xc
+
+#define VIA_CRDR_DMASTART       0x10
+/*0x14-0xFF reserved*/
+
+/*
+ * PCI Control Registers
+ */
+
+/*0x0 - 0x1 reserved*/
+#define VIA_CRDR_PCICLKGATT     0x2
+/*
+ * SFTRST :
+ * 0 : Soft reset all the controller and it will be de-asserted automatically
+ * 1 : Soft reset is de-asserted
+ */
+#define VIA_CRDR_PCICLKGATT_SFTRST      0x01
+/*
+ * 3V3 : Pad power select
+ * 0 : 1.8V
+ * 1 : 3.3V
+ * NOTE : No mater what the actual value should be, this bit always
+ * read as 0. This is a hardware bug.
+ */
+#define VIA_CRDR_PCICLKGATT_3V3 0x10
+/*
+ * PAD_PWRON : Pad Power on/off select
+ * 0 : Power off
+ * 1 : Power on
+  * NOTE : No mater what the actual value should be, this bit always
+ * read as 0. This is a hardware bug.
+ */
+#define VIA_CRDR_PCICLKGATT_PAD_PWRON   0x20
+
+#define VIA_CRDR_PCISDCCLK      0x5
+
+#define VIA_CRDR_PCIDMACLK      0x7
+#define VIA_CRDR_PCIDMACLK_SDC          0x2
+
+#define VIA_CRDR_PCIINTCTRL     0x8
+#define VIA_CRDR_PCIINTCTRL_SDCIRQEN    0x04
+
+#define VIA_CRDR_PCIINTSTATUS   0x9
+#define VIA_CRDR_PCIINTSTATUS_SDC       0x04
+
+#define  VIA_CRDR_PCITMOCTRL    0xa
+#define VIA_CRDR_PCITMOCTRL_NO          0x0
+#define VIA_CRDR_PCITMOCTRL_32US        0x1
+#define VIA_CRDR_PCITMOCTRL_256US       0x2
+#define VIA_CRDR_PCITMOCTRL_1024US      0x3
+#define VIA_CRDR_PCITMOCTRL_256MS       0x4
+#define VIA_CRDR_PCITMOCTRL_512MS       0x5
+#define VIA_CRDR_PCITMOCTRL_1024MS      0x6
+
+/*0xB-0xFF reserved*/
+
+enum PCI_HOST_CLK_CONTROL {
+        PCI_CLK_375K = 0x03,
+        PCI_CLK_8M = 0x04,
+        PCI_CLK_12M = 0x00,
+        PCI_CLK_16M = 0x05,
+        PCI_CLK_24M = 0x01,
+        PCI_CLK_33M = 0x06,
+        PCI_CLK_48M = 0x02
+};
+
+struct sdhcreg {
+        u32 sdcontrol_reg;
+        u32 sdcmdarg_reg;
+        u32 sdbusmode_reg;
+        u32 sdblklen_reg;
+        u32 sdresp_reg[4];
+        u32 sdcurblkcnt_reg;
+        u32 sdintmask_reg;
+        u32 sdstatus_reg;
+        u32 sdrsptmo_reg;
+        u32 sdclksel_reg;
+        u32 sdextctrl_reg;
+};
+
+struct pcictrlreg {
+        u8 reserve[2];
+        u8 pciclkgat_reg;
+        u8 pcinfcclk_reg;
+        u8 pcimscclk_reg;
+        u8 pcisdclk_reg;
+        u8 pcicaclk_reg;
+        u8 pcidmaclk_reg;
+        u8 pciintctrl_reg;
+        u8 pciintstatus_reg;
+        u8 pcitmoctrl_reg;
+        u8 Resv;
+};
+
+struct via_crdr_mmc_host {
+        struct mmc_host *mmc;
+        struct mmc_request *mrq;
+        struct mmc_command *cmd;
+        struct mmc_data *data;
+
+        void __iomem *mmiobase;
+        void __iomem *sdhc_mmiobase;
+        void __iomem *ddma_mmiobase;
+        void __iomem *pcictrl_mmiobase;
+
+        struct pcictrlreg pm_pcictrl_reg;
+        struct sdhcreg pm_sdhc_reg;
+
+        struct work_struct carddet_work;
+        struct tasklet_struct finish_tasklet;
+
+        struct timer_list timer;
+        spinlock_t lock;
+        u8 power;
+        int reject;
+        unsigned int quirks;
+};
+
+/* some devices need a very long delay for power to stabilize */
+#define VIA_CRDR_QUIRK_300MS_PWRDELAY   0x0001
+
+static struct pci_device_id via_ids[] = {
+        {PCI_VENDOR_ID_VIA, PCI_DEVICE_ID_VIA_9530,
+          PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0,},
+        {0,}
+};
+
+MODULE_DEVICE_TABLE(pci, via_ids);
+
+static void via_print_sdchc(struct via_crdr_mmc_host *host)
+{
+        void __iomem *addrbase = host->sdhc_mmiobase;
+
+        pr_debug("SDC MMIO Registers:\n");
+        pr_debug("SDCONTROL=%08x, SDCMDARG=%08x, SDBUSMODE=%08x\n",
+                 readl(addrbase + VIA_CRDR_SDCTRL),
+                 readl(addrbase + VIA_CRDR_SDCARG),
+                 readl(addrbase + VIA_CRDR_SDBUSMODE));
+        pr_debug("SDBLKLEN=%08x, SDCURBLKCNT=%08x, SDINTMASK=%08x\n",
+                 readl(addrbase + VIA_CRDR_SDBLKLEN),
+                 readl(addrbase + VIA_CRDR_SDCURBLKCNT),
+                 readl(addrbase + VIA_CRDR_SDINTMASK));
+        pr_debug("SDSTATUS=%08x, SDCLKSEL=%08x, SDEXTCTRL=%08x\n",
+                 readl(addrbase + VIA_CRDR_SDSTATUS),
+                 readl(addrbase + VIA_CRDR_SDCLKSEL),
+                 readl(addrbase + VIA_CRDR_SDEXTCTRL));
+}
+
+static void via_print_pcictrl(struct via_crdr_mmc_host *host)
+{
+        void __iomem *addrbase = host->pcictrl_mmiobase;
+
+        pr_debug("PCI Control Registers:\n");
+        pr_debug("PCICLKGATT=%02x, PCISDCCLK=%02x, PCIDMACLK=%02x\n",
+                 readb(addrbase + VIA_CRDR_PCICLKGATT),
+                 readb(addrbase + VIA_CRDR_PCISDCCLK),
+                 readb(addrbase + VIA_CRDR_PCIDMACLK));
+        pr_debug("PCIINTCTRL=%02x, PCIINTSTATUS=%02x\n",
+                 readb(addrbase + VIA_CRDR_PCIINTCTRL),
+                 readb(addrbase + VIA_CRDR_PCIINTSTATUS));
+}
+
+static void via_save_pcictrlreg(struct via_crdr_mmc_host *host)
+{
+        struct pcictrlreg *pm_pcictrl_reg;
+        void __iomem *addrbase;
+
+        pm_pcictrl_reg = &(host->pm_pcictrl_reg);
+        addrbase = host->pcictrl_mmiobase;
+
+        pm_pcictrl_reg->pciclkgat_reg = readb(addrbase + VIA_CRDR_PCICLKGATT);
+        pm_pcictrl_reg->pciclkgat_reg |=
+                VIA_CRDR_PCICLKGATT_3V3 | VIA_CRDR_PCICLKGATT_PAD_PWRON;
+        pm_pcictrl_reg->pcisdclk_reg = readb(addrbase + VIA_CRDR_PCISDCCLK);
+        pm_pcictrl_reg->pcidmaclk_reg = readb(addrbase + VIA_CRDR_PCIDMACLK);
+        pm_pcictrl_reg->pciintctrl_reg = readb(addrbase + VIA_CRDR_PCIINTCTRL);
+        pm_pcictrl_reg->pciintstatus_reg =
+                readb(addrbase + VIA_CRDR_PCIINTSTATUS);
+        pm_pcictrl_reg->pcitmoctrl_reg = readb(addrbase + VIA_CRDR_PCITMOCTRL);
+}
+
+static void via_restore_pcictrlreg(struct via_crdr_mmc_host *host)
+{
+        struct pcictrlreg *pm_pcictrl_reg;
+        void __iomem *addrbase;
+
+        pm_pcictrl_reg = &(host->pm_pcictrl_reg);
+        addrbase = host->pcictrl_mmiobase;
+
+        writeb(pm_pcictrl_reg->pciclkgat_reg, addrbase + VIA_CRDR_PCICLKGATT);
+        writeb(pm_pcictrl_reg->pcisdclk_reg, addrbase + VIA_CRDR_PCISDCCLK);
+        writeb(pm_pcictrl_reg->pcidmaclk_reg, addrbase + VIA_CRDR_PCIDMACLK);
+        writeb(pm_pcictrl_reg->pciintctrl_reg, addrbase + VIA_CRDR_PCIINTCTRL);
+        writeb(pm_pcictrl_reg->pciintstatus_reg,
+                addrbase + VIA_CRDR_PCIINTSTATUS);
+        writeb(pm_pcictrl_reg->pcitmoctrl_reg, addrbase + VIA_CRDR_PCITMOCTRL);
+}
+
+static void via_save_sdcreg(struct via_crdr_mmc_host *host)
+{
+        struct sdhcreg *pm_sdhc_reg;
+        void __iomem *addrbase;
+
+        pm_sdhc_reg = &(host->pm_sdhc_reg);
+        addrbase = host->sdhc_mmiobase;
+
+        pm_sdhc_reg->sdcontrol_reg = readl(addrbase + VIA_CRDR_SDCTRL);
+        pm_sdhc_reg->sdcmdarg_reg = readl(addrbase + VIA_CRDR_SDCARG);
+        pm_sdhc_reg->sdbusmode_reg = readl(addrbase + VIA_CRDR_SDBUSMODE);
+        pm_sdhc_reg->sdblklen_reg = readl(addrbase + VIA_CRDR_SDBLKLEN);
+        pm_sdhc_reg->sdcurblkcnt_reg = readl(addrbase + VIA_CRDR_SDCURBLKCNT);
+        pm_sdhc_reg->sdintmask_reg = readl(addrbase + VIA_CRDR_SDINTMASK);
+        pm_sdhc_reg->sdstatus_reg = readl(addrbase + VIA_CRDR_SDSTATUS);
+        pm_sdhc_reg->sdrsptmo_reg = readl(addrbase + VIA_CRDR_SDRSPTMO);
+        pm_sdhc_reg->sdclksel_reg = readl(addrbase + VIA_CRDR_SDCLKSEL);
+        pm_sdhc_reg->sdextctrl_reg = readl(addrbase + VIA_CRDR_SDEXTCTRL);
+}
+
+static void via_restore_sdcreg(struct via_crdr_mmc_host *host)
+{
+        struct sdhcreg *pm_sdhc_reg;
+        void __iomem *addrbase;
+
+        pm_sdhc_reg = &(host->pm_sdhc_reg);
+        addrbase = host->sdhc_mmiobase;
+
+        writel(pm_sdhc_reg->sdcontrol_reg, addrbase + VIA_CRDR_SDCTRL);
+        writel(pm_sdhc_reg->sdcmdarg_reg, addrbase + VIA_CRDR_SDCARG);
+        writel(pm_sdhc_reg->sdbusmode_reg, addrbase + VIA_CRDR_SDBUSMODE);
+        writel(pm_sdhc_reg->sdblklen_reg, addrbase + VIA_CRDR_SDBLKLEN);
+        writel(pm_sdhc_reg->sdcurblkcnt_reg, addrbase + VIA_CRDR_SDCURBLKCNT);
+        writel(pm_sdhc_reg->sdintmask_reg, addrbase + VIA_CRDR_SDINTMASK);
+        writel(pm_sdhc_reg->sdstatus_reg, addrbase + VIA_CRDR_SDSTATUS);
+        writel(pm_sdhc_reg->sdrsptmo_reg, addrbase + VIA_CRDR_SDRSPTMO);
+        writel(pm_sdhc_reg->sdclksel_reg, addrbase + VIA_CRDR_SDCLKSEL);
+        writel(pm_sdhc_reg->sdextctrl_reg, addrbase + VIA_CRDR_SDEXTCTRL);
+}
+
+static void via_pwron_sleep(struct via_crdr_mmc_host *sdhost)
+{
+        if (sdhost->quirks & VIA_CRDR_QUIRK_300MS_PWRDELAY)
+                msleep(300);
+        else
+                msleep(3);
+}
+
+static void via_set_ddma(struct via_crdr_mmc_host *host,
+                         dma_addr_t dmaaddr, u32 count, int dir, int enirq)
+{
+        void __iomem *addrbase;
+        u32 ctrl_data = 0;
+
+        if (enirq)
+                ctrl_data |= VIA_CRDR_DMACTRL_ENIRQ;
+
+        if (dir)
+                ctrl_data |= VIA_CRDR_DMACTRL_DIR;
+
+        addrbase = host->ddma_mmiobase;
+
+        writel(dmaaddr, addrbase + VIA_CRDR_DMABASEADD);
+        writel(count, addrbase + VIA_CRDR_DMACOUNTER);
+        writel(ctrl_data, addrbase + VIA_CRDR_DMACTRL);
+        writel(0x01, addrbase + VIA_CRDR_DMASTART);
+
+        /* It seems that our DMA can not work normally with 375kHz clock */
+        /* FIXME: don't brute-force 8MHz but use PIO at 375kHz !! */
+        addrbase = host->pcictrl_mmiobase;
+        if (readb(addrbase + VIA_CRDR_PCISDCCLK) == PCI_CLK_375K) {
+                dev_info(host->mmc->parent, "forcing card speed to 8MHz\n");
+                writeb(PCI_CLK_8M, addrbase + VIA_CRDR_PCISDCCLK);
+        }
+}
+
+static void via_sdc_preparedata(struct via_crdr_mmc_host *host,
+                                struct mmc_data *data)
+{
+        void __iomem *addrbase;
+        u32 blk_reg;
+        int count;
+
+        WARN_ON(host->data);
+
+        /* Sanity checks */
+        BUG_ON(data->blksz > host->mmc->max_blk_size);
+        BUG_ON(data->blocks > host->mmc->max_blk_count);
+
+        host->data = data;
+
+        count = dma_map_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
+                ((data->flags & MMC_DATA_READ) ?
+                PCI_DMA_FROMDEVICE : PCI_DMA_TODEVICE));
+        BUG_ON(count != 1);
+
+        via_set_ddma(host, sg_dma_address(data->sg), sg_dma_len(data->sg),
+                (data->flags & MMC_DATA_WRITE) ? 1 : 0, 1);
+
+        addrbase = host->sdhc_mmiobase;
+
+        blk_reg = data->blksz - 1;
+        blk_reg |= VIA_CRDR_SDBLKLEN_GPIDET | VIA_CRDR_SDBLKLEN_INTEN;
+        blk_reg |= (data->blocks) << 16;
+
+        writel(blk_reg, addrbase + VIA_CRDR_SDBLKLEN);
+}
+
+static void via_sdc_get_response(struct via_crdr_mmc_host *host,
+                                 struct mmc_command *cmd)
+{
+        void __iomem *addrbase = host->sdhc_mmiobase;
+        u32 dwdata0 = readl(addrbase + VIA_CRDR_SDRESP0);
+        u32 dwdata1 = readl(addrbase + VIA_CRDR_SDRESP1);
+        u32 dwdata2 = readl(addrbase + VIA_CRDR_SDRESP2);
+        u32 dwdata3 = readl(addrbase + VIA_CRDR_SDRESP3);
+
+        if (cmd->flags & MMC_RSP_136) {
+                cmd->resp[0] = ((u8) (dwdata1)) |
+                    (((u8) (dwdata0 >> 24)) << 8) |
+                    (((u8) (dwdata0 >> 16)) << 16) |
+                    (((u8) (dwdata0 >> 8)) << 24);
+
+                cmd->resp[1] = ((u8) (dwdata2)) |
+                    (((u8) (dwdata1 >> 24)) << 8) |
+                    (((u8) (dwdata1 >> 16)) << 16) |
+                    (((u8) (dwdata1 >> 8)) << 24);
+
+                cmd->resp[2] = ((u8) (dwdata3)) |
+                    (((u8) (dwdata2 >> 24)) << 8) |
+                    (((u8) (dwdata2 >> 16)) << 16) |
+                    (((u8) (dwdata2 >> 8)) << 24);
+
+                cmd->resp[3] = 0xff |
+                    ((((u8) (dwdata3 >> 24))) << 8) |
+                    (((u8) (dwdata3 >> 16)) << 16) |
+                    (((u8) (dwdata3 >> 8)) << 24);
+        } else {
+                dwdata0 >>= 8;
+                cmd->resp[0] = ((dwdata0 & 0xff) << 24) |
+                    (((dwdata0 >> 8) & 0xff) << 16) |
+                    (((dwdata0 >> 16) & 0xff) << 8) | (dwdata1 & 0xff);
+
+                dwdata1 >>= 8;
+                cmd->resp[1] = ((dwdata1 & 0xff) << 24) |
+                    (((dwdata1 >> 8) & 0xff) << 16) |
+                    (((dwdata1 >> 16) & 0xff) << 8);
+        }
+}
+
+static void via_sdc_send_command(struct via_crdr_mmc_host *host,
+                                 struct mmc_command *cmd)
+{
+        void __iomem *addrbase;
+        struct mmc_data *data;
+        u32 cmdctrl = 0;
+
+        WARN_ON(host->cmd);
+
+        data = cmd->data;
+        mod_timer(&host->timer, jiffies + HZ);
+        host->cmd = cmd;
+
+        /*Command index*/
+        cmdctrl = cmd->opcode << 8;
+
+        /*Response type*/
+        switch (mmc_resp_type(cmd)) {
+        case MMC_RSP_NONE:
+                cmdctrl |= VIA_CRDR_SDCTRL_RSP_NONE;
+                break;
+        case MMC_RSP_R1:
+                cmdctrl |= VIA_CRDR_SDCTRL_RSP_R1;
+                break;
+        case MMC_RSP_R1B:
+                cmdctrl |= VIA_CRDR_SDCTRL_RSP_R1B;
+                break;
+        case MMC_RSP_R2:
+                cmdctrl |= VIA_CRDR_SDCTRL_RSP_R2;
+                break;
+        case MMC_RSP_R3:
+                cmdctrl |= VIA_CRDR_SDCTRL_RSP_R3;
+                break;
+        default:
+                pr_err("%s: cmd->flag is not valid\n", mmc_hostname(host->mmc));
+                break;
+        }
+
+        if (!(cmd->data))
+                goto nodata;
+
+        via_sdc_preparedata(host, data);
+
+        /*Command control*/
+        if (data->blocks > 1) {
+                if (data->flags & MMC_DATA_WRITE) {
+                        cmdctrl |= VIA_CRDR_SDCTRL_WRITE;
+                        cmdctrl |= VIA_CRDR_SDCTRL_MULTI_WR;
+                } else {
+                        cmdctrl |= VIA_CRDR_SDCTRL_MULTI_RD;
+                }
+        } else {
+                if (data->flags & MMC_DATA_WRITE) {
+                        cmdctrl |= VIA_CRDR_SDCTRL_WRITE;
+                        cmdctrl |= VIA_CRDR_SDCTRL_SINGLE_WR;
+                } else {
+                        cmdctrl |= VIA_CRDR_SDCTRL_SINGLE_RD;
+                }
+        }
+
+nodata:
+        if (cmd == host->mrq->stop)
+                cmdctrl |= VIA_CRDR_SDCTRL_STOP;
+
+        cmdctrl |= VIA_CRDR_SDCTRL_START;
+
+        addrbase = host->sdhc_mmiobase;
+        writel(cmd->arg, addrbase + VIA_CRDR_SDCARG);
+        writel(cmdctrl, addrbase + VIA_CRDR_SDCTRL);
+}
+
+static void via_sdc_finish_data(struct via_crdr_mmc_host *host)
+{
+        struct mmc_data *data;
+
+        BUG_ON(!host->data);
+
+        data = host->data;
+        host->data = NULL;
+
+        if (data->error)
+                data->bytes_xfered = 0;
+        else
+                data->bytes_xfered = data->blocks * data->blksz;
+
+        dma_unmap_sg(mmc_dev(host->mmc), data->sg, data->sg_len,
+                ((data->flags & MMC_DATA_READ) ?
+                PCI_DMA_FROMDEVICE : PCI_DMA_TODEVICE));
+
+        if (data->stop)
+                via_sdc_send_command(host, data->stop);
+        else
+                tasklet_schedule(&host->finish_tasklet);
+}
+
+static void via_sdc_finish_command(struct via_crdr_mmc_host *host)
+{
+        via_sdc_get_response(host, host->cmd);
+
+        host->cmd->error = 0;
+
+        if (!host->cmd->data)
+                tasklet_schedule(&host->finish_tasklet);
+
+        host->cmd = NULL;
+}
+
+static void via_sdc_request(struct mmc_host *mmc, struct mmc_request *mrq)
+{
+        void __iomem *addrbase;
+        struct via_crdr_mmc_host *host;
+        unsigned long flags;
+        u16 status;
+
+        host = mmc_priv(mmc);
+
+        spin_lock_irqsave(&host->lock, flags);
+
+        addrbase = host->pcictrl_mmiobase;
+        writeb(VIA_CRDR_PCIDMACLK_SDC, addrbase + VIA_CRDR_PCIDMACLK);
+
+        status = readw(host->sdhc_mmiobase + VIA_CRDR_SDSTATUS);
+        status &= VIA_CRDR_SDSTS_W1C_MASK;
+        writew(status, host->sdhc_mmiobase + VIA_CRDR_SDSTATUS);
+
+        WARN_ON(host->mrq != NULL);
+        host->mrq = mrq;
+
+        status = readw(host->sdhc_mmiobase + VIA_CRDR_SDSTATUS);
+        if (!(status & VIA_CRDR_SDSTS_SLOTG) || host->reject) {
+                host->mrq->cmd->error = -ENOMEDIUM;
+                tasklet_schedule(&host->finish_tasklet);
+        } else {
+                via_sdc_send_command(host, mrq->cmd);
+        }
+
+        mmiowb();
+        spin_unlock_irqrestore(&host->lock, flags);
+}
+
+static void via_sdc_set_power(struct via_crdr_mmc_host *host,
+                              unsigned short power, unsigned int on)
+{
+        unsigned long flags;
+        u8 gatt;
+
+        spin_lock_irqsave(&host->lock, flags);
+
+        host->power = (1 << power);
+
+        gatt = readb(host->pcictrl_mmiobase + VIA_CRDR_PCICLKGATT);
+        if (host->power == MMC_VDD_165_195)
+                gatt &= ~VIA_CRDR_PCICLKGATT_3V3;
+        else
+                gatt |= VIA_CRDR_PCICLKGATT_3V3;
+        if (on)
+                gatt |= VIA_CRDR_PCICLKGATT_PAD_PWRON;
+        else
+                gatt &= ~VIA_CRDR_PCICLKGATT_PAD_PWRON;
+        writeb(gatt, host->pcictrl_mmiobase + VIA_CRDR_PCICLKGATT);
+
+        mmiowb();
+        spin_unlock_irqrestore(&host->lock, flags);
+
+        via_pwron_sleep(host);
+}
+
+static void via_sdc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+        struct via_crdr_mmc_host *host;
+        unsigned long flags;
+        void __iomem *addrbase;
+        u32 org_data, sdextctrl;
+        u8 clock;
+
+        host = mmc_priv(mmc);
+
+        spin_lock_irqsave(&host->lock, flags);
+
+        addrbase = host->sdhc_mmiobase;
+        org_data = readl(addrbase + VIA_CRDR_SDBUSMODE);
+        sdextctrl = readl(addrbase + VIA_CRDR_SDEXTCTRL);
+
+        if (ios->bus_width == MMC_BUS_WIDTH_1)
+                org_data &= ~VIA_CRDR_SDMODE_4BIT;
+        else
+                org_data |= VIA_CRDR_SDMODE_4BIT;
+
+        if (ios->power_mode == MMC_POWER_OFF)
+                org_data &= ~VIA_CRDR_SDMODE_CLK_ON;
+        else
+                org_data |= VIA_CRDR_SDMODE_CLK_ON;
+
+        if (ios->timing == MMC_TIMING_SD_HS)
+                sdextctrl |= VIA_CRDR_SDEXTCTRL_HISPD;
+        else
+                sdextctrl &= ~VIA_CRDR_SDEXTCTRL_HISPD;
+
+        writel(org_data, addrbase + VIA_CRDR_SDBUSMODE);
+        writel(sdextctrl, addrbase + VIA_CRDR_SDEXTCTRL);
+
+        if (ios->clock >= 48000000)
+                clock = PCI_CLK_48M;
+        else if (ios->clock >= 33000000)
+                clock = PCI_CLK_33M;
+        else if (ios->clock >= 24000000)
+                clock = PCI_CLK_24M;
+        else if (ios->clock >= 16000000)
+                clock = PCI_CLK_16M;
+        else if (ios->clock >= 12000000)
+                clock = PCI_CLK_12M;
+        else if (ios->clock >=  8000000)
+                clock = PCI_CLK_8M;
+        else
+                clock = PCI_CLK_375K;
+
+        addrbase = host->pcictrl_mmiobase;
+        if (readb(addrbase + VIA_CRDR_PCISDCCLK) != clock)
+                writeb(clock, addrbase + VIA_CRDR_PCISDCCLK);
+
+        mmiowb();
+        spin_unlock_irqrestore(&host->lock, flags);
+
+        if (ios->power_mode != MMC_POWER_OFF)
+                via_sdc_set_power(host, ios->vdd, 1);
+        else
+                via_sdc_set_power(host, ios->vdd, 0);
+}
+
+static int via_sdc_get_ro(struct mmc_host *mmc)
+{
+        struct via_crdr_mmc_host *host;
+        unsigned long flags;
+        u16 status;
+
+        host = mmc_priv(mmc);
+
+        spin_lock_irqsave(&host->lock, flags);
+
+        status = readw(host->sdhc_mmiobase + VIA_CRDR_SDSTATUS);
+
+        spin_unlock_irqrestore(&host->lock, flags);
+
+        return !(status & VIA_CRDR_SDSTS_WP);
+}
+
+static const struct mmc_host_ops via_sdc_ops = {
+        .request = via_sdc_request,
+        .set_ios = via_sdc_set_ios,
+        .get_ro = via_sdc_get_ro,
+};
+
+static void via_reset_pcictrl(struct via_crdr_mmc_host *host)
+{
+        void __iomem *addrbase;
+        unsigned long flags;
+        u8 gatt;
+
+        addrbase = host->pcictrl_mmiobase;
+
+        spin_lock_irqsave(&host->lock, flags);
+
+        via_save_pcictrlreg(host);
+        via_save_sdcreg(host);
+
+        spin_unlock_irqrestore(&host->lock, flags);
+
+        gatt = VIA_CRDR_PCICLKGATT_PAD_PWRON;
+        if (host->power == MMC_VDD_165_195)
+                gatt &= VIA_CRDR_PCICLKGATT_3V3;
+        else
+                gatt |= VIA_CRDR_PCICLKGATT_3V3;
+        writeb(gatt, host->pcictrl_mmiobase + VIA_CRDR_PCICLKGATT);
+        via_pwron_sleep(host);
+        gatt |= VIA_CRDR_PCICLKGATT_SFTRST;
+        writeb(gatt, host->pcictrl_mmiobase + VIA_CRDR_PCICLKGATT);
+        msleep(3);
+
+        spin_lock_irqsave(&host->lock, flags);
+
+        via_restore_pcictrlreg(host);
+        via_restore_sdcreg(host);
+
+        mmiowb();
+        spin_unlock_irqrestore(&host->lock, flags);
+}
+
+static void via_sdc_cmd_isr(struct via_crdr_mmc_host *host, u16 intmask)
+{
+        BUG_ON(intmask == 0);
+
+        if (!host->cmd) {
+                pr_err("%s: Got command interrupt 0x%x even "
+                       "though no command operation was in progress.\n",
+                       mmc_hostname(host->mmc), intmask);
+                return;
+        }
+
+        if (intmask & VIA_CRDR_SDSTS_CRTO)
+                host->cmd->error = -ETIMEDOUT;
+        else if (intmask & VIA_CRDR_SDSTS_SC)
+                host->cmd->error = -EILSEQ;
+
+        if (host->cmd->error)
+                tasklet_schedule(&host->finish_tasklet);
+        else if (intmask & VIA_CRDR_SDSTS_CRD)
+                via_sdc_finish_command(host);
+}
+
+static void via_sdc_data_isr(struct via_crdr_mmc_host *host, u16 intmask)
+{
+        BUG_ON(intmask == 0);
+
+        if (intmask & VIA_CRDR_SDSTS_DT)
+                host->data->error = -ETIMEDOUT;
+        else if (intmask & (VIA_CRDR_SDSTS_RC | VIA_CRDR_SDSTS_WC))
+                host->data->error = -EILSEQ;
+
+        via_sdc_finish_data(host);
+}
+
+static irqreturn_t via_sdc_isr(int irq, void *dev_id)
+{
+        struct via_crdr_mmc_host *sdhost = dev_id;
+        void __iomem *addrbase;
+        u8 pci_status;
+        u16 sd_status;
+        irqreturn_t result;
+
+        if (!sdhost)
+                return IRQ_NONE;
+
+        spin_lock(&sdhost->lock);
+
+        addrbase = sdhost->pcictrl_mmiobase;
+        pci_status = readb(addrbase + VIA_CRDR_PCIINTSTATUS);
+        if (!(pci_status & VIA_CRDR_PCIINTSTATUS_SDC)) {
+                result = IRQ_NONE;
+                goto out;
+        }
+
+        addrbase = sdhost->sdhc_mmiobase;
+        sd_status = readw(addrbase + VIA_CRDR_SDSTATUS);
+        sd_status &= VIA_CRDR_SDSTS_INT_MASK;
+        sd_status &= ~VIA_CRDR_SDSTS_IGN_MASK;
+        if (!sd_status) {
+                result = IRQ_NONE;
+                goto out;
+        }
+
+        if (sd_status & VIA_CRDR_SDSTS_CIR) {
+                writew(sd_status & VIA_CRDR_SDSTS_CIR,
+                        addrbase + VIA_CRDR_SDSTATUS);
+
+                schedule_work(&sdhost->carddet_work);
+        }
+
+        sd_status &= ~VIA_CRDR_SDSTS_CIR;
+        if (sd_status & VIA_CRDR_SDSTS_CMD_MASK) {
+                writew(sd_status & VIA_CRDR_SDSTS_CMD_MASK,
+                        addrbase + VIA_CRDR_SDSTATUS);
+                via_sdc_cmd_isr(sdhost, sd_status & VIA_CRDR_SDSTS_CMD_MASK);
+        }
+        if (sd_status & VIA_CRDR_SDSTS_DATA_MASK) {
+                writew(sd_status & VIA_CRDR_SDSTS_DATA_MASK,
+                        addrbase + VIA_CRDR_SDSTATUS);
+                via_sdc_data_isr(sdhost, sd_status & VIA_CRDR_SDSTS_DATA_MASK);
+        }
+
+        sd_status &= ~(VIA_CRDR_SDSTS_CMD_MASK | VIA_CRDR_SDSTS_DATA_MASK);
+        if (sd_status) {
+                pr_err("%s: Unexpected interrupt 0x%x\n",
+                       mmc_hostname(sdhost->mmc), sd_status);
+                writew(sd_status, addrbase + VIA_CRDR_SDSTATUS);
+        }
+
+        result = IRQ_HANDLED;
+
+        mmiowb();
+out:
+        spin_unlock(&sdhost->lock);
+
+        return result;
+}
+
+static void via_sdc_timeout(unsigned long ulongdata)
+{
+        struct via_crdr_mmc_host *sdhost;
+        unsigned long flags;
+
+        sdhost = (struct via_crdr_mmc_host *)ulongdata;
+
+        spin_lock_irqsave(&sdhost->lock, flags);
+
+        if (sdhost->mrq) {
+                pr_err("%s: Timeout waiting for hardware interrupt."
+                       "cmd:0x%x\n", mmc_hostname(sdhost->mmc),
+                       sdhost->mrq->cmd->opcode);
+
+                if (sdhost->data) {
+                        writel(VIA_CRDR_DMACTRL_SFTRST,
+                                sdhost->ddma_mmiobase + VIA_CRDR_DMACTRL);
+                        sdhost->data->error = -ETIMEDOUT;
+                        via_sdc_finish_data(sdhost);
+                } else {
+                        if (sdhost->cmd)
+                                sdhost->cmd->error = -ETIMEDOUT;
+                        else
+                                sdhost->mrq->cmd->error = -ETIMEDOUT;
+                        tasklet_schedule(&sdhost->finish_tasklet);
+                }
+        }
+
+        mmiowb();
+        spin_unlock_irqrestore(&sdhost->lock, flags);
+}
+
+static void via_sdc_tasklet_finish(unsigned long param)
+{
+        struct via_crdr_mmc_host *host;
+        unsigned long flags;
+        struct mmc_request *mrq;
+
+        host = (struct via_crdr_mmc_host *)param;
+
+        spin_lock_irqsave(&host->lock, flags);
+
+        del_timer(&host->timer);
+        mrq = host->mrq;
+        host->mrq = NULL;
+        host->cmd = NULL;
+        host->data = NULL;
+
+        spin_unlock_irqrestore(&host->lock, flags);
+
+        mmc_request_done(host->mmc, mrq);
+}
+
+static void via_sdc_card_detect(struct work_struct *work)
+{
+        struct via_crdr_mmc_host *host;
+        void __iomem *addrbase;
+        unsigned long flags;
+        u16 status;
+
+        host = container_of(work, struct via_crdr_mmc_host, carddet_work);
+
+        addrbase = host->ddma_mmiobase;
+        writel(VIA_CRDR_DMACTRL_SFTRST, addrbase + VIA_CRDR_DMACTRL);
+
+        spin_lock_irqsave(&host->lock, flags);
+
+        addrbase = host->pcictrl_mmiobase;
+        writeb(VIA_CRDR_PCIDMACLK_SDC, addrbase + VIA_CRDR_PCIDMACLK);
+
+        addrbase = host->sdhc_mmiobase;
+        status = readw(addrbase + VIA_CRDR_SDSTATUS);
+        if (!(status & VIA_CRDR_SDSTS_SLOTG)) {
+                if (host->mrq) {
+                        pr_err("%s: Card removed during transfer!\n",
+                               mmc_hostname(host->mmc));
+                        host->mrq->cmd->error = -ENOMEDIUM;
+                        tasklet_schedule(&host->finish_tasklet);
+                }
+
+                mmiowb();
+                spin_unlock_irqrestore(&host->lock, flags);
+
+                via_reset_pcictrl(host);
+
+                spin_lock_irqsave(&host->lock, flags);
+        }
+
+        mmiowb();
+        spin_unlock_irqrestore(&host->lock, flags);
+
+        via_print_pcictrl(host);
+        via_print_sdchc(host);
+
+        mmc_detect_change(host->mmc, msecs_to_jiffies(500));
+}
+
+static void via_init_mmc_host(struct via_crdr_mmc_host *host)
+{
+        struct mmc_host *mmc = host->mmc;
+        void __iomem *addrbase;
+        u32 lenreg;
+        u32 status;
+
+        init_timer(&host->timer);
+        host->timer.data = (unsigned long)host;
+        host->timer.function = via_sdc_timeout;
+
+        spin_lock_init(&host->lock);
+
+        mmc->f_min = VIA_CRDR_MIN_CLOCK;
+        mmc->f_max = VIA_CRDR_MAX_CLOCK;
+        mmc->ocr_avail = MMC_VDD_32_33 | MMC_VDD_33_34 | MMC_VDD_165_195;
+        mmc->caps = MMC_CAP_4_BIT_DATA | MMC_CAP_SD_HIGHSPEED;
+        mmc->ops = &via_sdc_ops;
+
+        /*Hardware cannot do scatter lists*/
+        mmc->max_hw_segs = 1;
+        mmc->max_phys_segs = 1;
+
+        mmc->max_blk_size = VIA_CRDR_MAX_BLOCK_LENGTH;
+        mmc->max_blk_count = VIA_CRDR_MAX_BLOCK_COUNT;
+
+        mmc->max_seg_size = mmc->max_blk_size * mmc->max_blk_count;
+        mmc->max_req_size = mmc->max_seg_size;
+
+        INIT_WORK(&host->carddet_work, via_sdc_card_detect);
+
+        tasklet_init(&host->finish_tasklet, via_sdc_tasklet_finish,
+                     (unsigned long)host);
+
+        addrbase = host->sdhc_mmiobase;
+        writel(0x0, addrbase + VIA_CRDR_SDINTMASK);
+        msleep(1);
+
+        lenreg = VIA_CRDR_SDBLKLEN_GPIDET | VIA_CRDR_SDBLKLEN_INTEN;
+        writel(lenreg, addrbase + VIA_CRDR_SDBLKLEN);
+
+        status = readw(addrbase + VIA_CRDR_SDSTATUS);
+        status &= VIA_CRDR_SDSTS_W1C_MASK;
+        writew(status, addrbase + VIA_CRDR_SDSTATUS);
+
+        status = readw(addrbase + VIA_CRDR_SDSTATUS2);
+        status |= VIA_CRDR_SDSTS_CFE;
+        writew(status, addrbase + VIA_CRDR_SDSTATUS2);
+
+        writeb(0x0, addrbase + VIA_CRDR_SDEXTCTRL);
+
+        writel(VIA_CRDR_SDACTIVE_INTMASK, addrbase + VIA_CRDR_SDINTMASK);
+        msleep(1);
+}
+
+static int __devinit via_sd_probe(struct pci_dev *pcidev,
+                                    const struct pci_device_id *id)
+{
+        struct mmc_host *mmc;
+        struct via_crdr_mmc_host *sdhost;
+        u32 base, len;
+        u8 rev, gatt;
+        int ret;
+
+        pci_read_config_byte(pcidev, PCI_CLASS_REVISION, &rev);
+        pr_info(DRV_NAME
+                ": VIA SDMMC controller found at %s [%04x:%04x] (rev %x)\n",
+                pci_name(pcidev), (int)pcidev->vendor, (int)pcidev->device,
+                (int)rev);
+
+        ret = pci_enable_device(pcidev);
+        if (ret)
+                return ret;
+
+        ret = pci_request_regions(pcidev, DRV_NAME);
+        if (ret)
+                goto disable;
+
+        pci_write_config_byte(pcidev, VIA_CRDR_PCI_WORK_MODE, 0);
+        pci_write_config_byte(pcidev, VIA_CRDR_PCI_DBG_MODE, 0);
+
+        mmc = mmc_alloc_host(sizeof(struct via_crdr_mmc_host), &pcidev->dev);
+        if (!mmc) {
+                ret = -ENOMEM;
+                goto release;
+        }
+
+        sdhost = mmc_priv(mmc);
+        sdhost->mmc = mmc;
+        dev_set_drvdata(&pcidev->dev, sdhost);
+
+        len = pci_resource_len(pcidev, 0);
+        base = pci_resource_start(pcidev, 0);
+        sdhost->mmiobase = ioremap_nocache(base, len);
+        if (!sdhost->mmiobase) {
+                ret = -ENOMEM;
+                goto free_mmc_host;
+        }
+
+        sdhost->sdhc_mmiobase =
+                sdhost->mmiobase + VIA_CRDR_SDC_OFF;
+        sdhost->ddma_mmiobase =
+                sdhost->mmiobase + VIA_CRDR_DDMA_OFF;
+        sdhost->pcictrl_mmiobase =
+                sdhost->mmiobase + VIA_CRDR_PCICTRL_OFF;
+
+        sdhost->power = MMC_VDD_165_195;
+
+        gatt = VIA_CRDR_PCICLKGATT_3V3 | VIA_CRDR_PCICLKGATT_PAD_PWRON;
+        writeb(gatt, sdhost->pcictrl_mmiobase + VIA_CRDR_PCICLKGATT);
+        via_pwron_sleep(sdhost);
+        gatt |= VIA_CRDR_PCICLKGATT_SFTRST;
+        writeb(gatt, sdhost->pcictrl_mmiobase + VIA_CRDR_PCICLKGATT);
+        msleep(3);
+
+        via_init_mmc_host(sdhost);
+
+        ret =
+            request_irq(pcidev->irq, via_sdc_isr, IRQF_SHARED, DRV_NAME,
+                        sdhost);
+        if (ret)
+                goto unmap;
+
+        writeb(VIA_CRDR_PCIINTCTRL_SDCIRQEN,
+               sdhost->pcictrl_mmiobase + VIA_CRDR_PCIINTCTRL);
+        writeb(VIA_CRDR_PCITMOCTRL_1024MS,
+               sdhost->pcictrl_mmiobase + VIA_CRDR_PCITMOCTRL);
+
+        /* device-specific quirks */
+        if (pcidev->subsystem_vendor == PCI_VENDOR_ID_LENOVO &&
+            pcidev->subsystem_device == 0x3891)
+                sdhost->quirks = VIA_CRDR_QUIRK_300MS_PWRDELAY;
+
+        mmc_add_host(mmc);
+
+        return 0;
+
+unmap:
+        iounmap(sdhost->mmiobase);
+free_mmc_host:
+        dev_set_drvdata(&pcidev->dev, NULL);
+        mmc_free_host(mmc);
+release:
+        pci_release_regions(pcidev);
+disable:
+        pci_disable_device(pcidev);
+
+        return ret;
+}
+
+static void __devexit via_sd_remove(struct pci_dev *pcidev)
+{
+        struct via_crdr_mmc_host *sdhost = pci_get_drvdata(pcidev);
+        unsigned long flags;
+        u8 gatt;
+
+        spin_lock_irqsave(&sdhost->lock, flags);
+
+        /* Ensure we don't accept more commands from mmc layer */
+        sdhost->reject = 1;
+
+        /* Disable generating further interrupts */
+        writeb(0x0, sdhost->pcictrl_mmiobase + VIA_CRDR_PCIINTCTRL);
+        mmiowb();
+
+        if (sdhost->mrq) {
+                printk(KERN_ERR "%s: Controller removed during "
+                        "transfer\n", mmc_hostname(sdhost->mmc));
+
+                /* make sure all DMA is stopped */
+                writel(VIA_CRDR_DMACTRL_SFTRST,
+                        sdhost->ddma_mmiobase + VIA_CRDR_DMACTRL);
+                mmiowb();
+                sdhost->mrq->cmd->error = -ENOMEDIUM;
+                if (sdhost->mrq->stop)
+                        sdhost->mrq->stop->error = -ENOMEDIUM;
+                tasklet_schedule(&sdhost->finish_tasklet);
+        }
+        spin_unlock_irqrestore(&sdhost->lock, flags);
+
+        mmc_remove_host(sdhost->mmc);
+
+        free_irq(pcidev->irq, sdhost);
+
+        del_timer_sync(&sdhost->timer);
+
+        tasklet_kill(&sdhost->finish_tasklet);
+
+        /* switch off power */
+        gatt = readb(sdhost->pcictrl_mmiobase + VIA_CRDR_PCICLKGATT);
+        gatt &= ~VIA_CRDR_PCICLKGATT_PAD_PWRON;
+        writeb(gatt, sdhost->pcictrl_mmiobase + VIA_CRDR_PCICLKGATT);
+
+        iounmap(sdhost->mmiobase);
+        dev_set_drvdata(&pcidev->dev, NULL);
+        mmc_free_host(sdhost->mmc);
+        pci_release_regions(pcidev);
+        pci_disable_device(pcidev);
+
+        pr_info(DRV_NAME
+                ": VIA SDMMC controller at %s [%04x:%04x] has been removed\n",
+                pci_name(pcidev), (int)pcidev->vendor, (int)pcidev->device);
+}
+
+#ifdef CONFIG_PM
+
+static void via_init_sdc_pm(struct via_crdr_mmc_host *host)
+{
+        struct sdhcreg *pm_sdhcreg;
+        void __iomem *addrbase;
+        u32 lenreg;
+        u16 status;
+
+        pm_sdhcreg = &(host->pm_sdhc_reg);
+        addrbase = host->sdhc_mmiobase;
+
+        writel(0x0, addrbase + VIA_CRDR_SDINTMASK);
+
+        lenreg = VIA_CRDR_SDBLKLEN_GPIDET | VIA_CRDR_SDBLKLEN_INTEN;
+        writel(lenreg, addrbase + VIA_CRDR_SDBLKLEN);
+
+        status = readw(addrbase + VIA_CRDR_SDSTATUS);
+        status &= VIA_CRDR_SDSTS_W1C_MASK;
+        writew(status, addrbase + VIA_CRDR_SDSTATUS);
+
+        status = readw(addrbase + VIA_CRDR_SDSTATUS2);
+        status |= VIA_CRDR_SDSTS_CFE;
+        writew(status, addrbase + VIA_CRDR_SDSTATUS2);
+
+        writel(pm_sdhcreg->sdcontrol_reg, addrbase + VIA_CRDR_SDCTRL);
+        writel(pm_sdhcreg->sdcmdarg_reg, addrbase + VIA_CRDR_SDCARG);
+        writel(pm_sdhcreg->sdintmask_reg, addrbase + VIA_CRDR_SDINTMASK);
+        writel(pm_sdhcreg->sdrsptmo_reg, addrbase + VIA_CRDR_SDRSPTMO);
+        writel(pm_sdhcreg->sdclksel_reg, addrbase + VIA_CRDR_SDCLKSEL);
+        writel(pm_sdhcreg->sdextctrl_reg, addrbase + VIA_CRDR_SDEXTCTRL);
+
+        via_print_pcictrl(host);
+        via_print_sdchc(host);
+}
+
+static int via_sd_suspend(struct pci_dev *pcidev, pm_message_t state)
+{
+        struct via_crdr_mmc_host *host;
+        int ret = 0;
+
+        host = pci_get_drvdata(pcidev);
+
+        via_save_pcictrlreg(host);
+        via_save_sdcreg(host);
+
+        ret = mmc_suspend_host(host->mmc, state);
+
+        pci_save_state(pcidev);
+        pci_enable_wake(pcidev, pci_choose_state(pcidev, state), 0);
+        pci_disable_device(pcidev);
+        pci_set_power_state(pcidev, pci_choose_state(pcidev, state));
+
+        return ret;
+}
+
+static int via_sd_resume(struct pci_dev *pcidev)
+{
+        struct via_crdr_mmc_host *sdhost;
+        int ret = 0;
+        u8 gatt;
+
+        sdhost = pci_get_drvdata(pcidev);
+
+        gatt = VIA_CRDR_PCICLKGATT_PAD_PWRON;
+        if (sdhost->power == MMC_VDD_165_195)
+                gatt &= ~VIA_CRDR_PCICLKGATT_3V3;
+        else
+                gatt |= VIA_CRDR_PCICLKGATT_3V3;
+        writeb(gatt, sdhost->pcictrl_mmiobase + VIA_CRDR_PCICLKGATT);
+        via_pwron_sleep(sdhost);
+        gatt |= VIA_CRDR_PCICLKGATT_SFTRST;
+        writeb(gatt, sdhost->pcictrl_mmiobase + VIA_CRDR_PCICLKGATT);
+        msleep(3);
+
+        msleep(100);
+
+        pci_set_power_state(pcidev, PCI_D0);
+        pci_restore_state(pcidev);
+        ret = pci_enable_device(pcidev);
+        if (ret)
+                return ret;
+
+        via_restore_pcictrlreg(sdhost);
+        via_init_sdc_pm(sdhost);
+
+        ret = mmc_resume_host(sdhost->mmc);
+
+        return ret;
+}
+
+#else /* CONFIG_PM */
+
+#define via_sd_suspend NULL
+#define via_sd_resume NULL
+
+#endif /* CONFIG_PM */
+
+static struct pci_driver via_sd_driver = {
+        .name = DRV_NAME,
+        .id_table = via_ids,
+        .probe = via_sd_probe,
+        .remove = __devexit_p(via_sd_remove),
+        .suspend = via_sd_suspend,
+        .resume = via_sd_resume,
+};
+
+static int __init via_sd_drv_init(void)
+{
+        pr_info(DRV_NAME ": VIA SD/MMC Card Reader driver "
+                "(C) 2008 VIA Technologies, Inc.\n");
+
+        return pci_register_driver(&via_sd_driver);
+}
+
+static void __exit via_sd_drv_exit(void)
+{
+        pci_unregister_driver(&via_sd_driver);
+}
+
+module_init(via_sd_drv_init);
+module_exit(via_sd_drv_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("VIA Technologies Inc.");
+MODULE_DESCRIPTION("VIA SD/MMC Card Interface driver");
diff --git a/drivers/mtd/chips/cfi_cmdset_0001.c b/drivers/mtd/chips/cfi_cmdset_0001.c
index c240454fd113..8664feebc93b 100644
--- a/drivers/mtd/chips/cfi_cmdset_0001.c
+++ b/drivers/mtd/chips/cfi_cmdset_0001.c
@@ -46,6 +46,7 @@
 #define MANUFACTURER_INTEL      0x0089
 #define I82802AB        0x00ad
 #define I82802AC        0x00ac
+#define PF38F4476       0x881c
 #define MANUFACTURER_ST         0x0020
 #define M50LPW080       0x002F
 #define M50FLW080A      0x0080
@@ -315,10 +316,20 @@ static struct cfi_fixup fixup_table[] = {
         { 0, 0, NULL, NULL }
 };
 
+static void cfi_fixup_major_minor(struct cfi_private *cfi,
+                                                struct cfi_pri_intelext *extp)
+{
+        if (cfi->mfr == MANUFACTURER_INTEL &&
+                        cfi->id == PF38F4476 && extp->MinorVersion == '3')
+                extp->MinorVersion = '1';
+}
+
 static inline struct cfi_pri_intelext *
 read_pri_intelext(struct map_info *map, __u16 adr)
 {
+        struct cfi_private *cfi = map->fldrv_priv;
         struct cfi_pri_intelext *extp;
+        unsigned int extra_size = 0;
         unsigned int extp_size = sizeof(*extp);
 
  again:
@@ -326,6 +337,8 @@ read_pri_intelext(struct map_info *map, __u16 adr)
         if (!extp)
                 return NULL;
 
+        cfi_fixup_major_minor(cfi, extp);
+
         if (extp->MajorVersion != '1' ||
             (extp->MinorVersion < '0' || extp->MinorVersion > '5')) {
                 printk(KERN_ERR "  Unknown Intel/Sharp Extended Query "
@@ -340,19 +353,24 @@ read_pri_intelext(struct map_info *map, __u16 adr)
         extp->BlkStatusRegMask = le16_to_cpu(extp->BlkStatusRegMask);
         extp->ProtRegAddr = le16_to_cpu(extp->ProtRegAddr);
 
-        if (extp->MajorVersion == '1' && extp->MinorVersion >= '3') {
-                unsigned int extra_size = 0;
-                int nb_parts, i;
+        if (extp->MinorVersion >= '0') {
+                extra_size = 0;
 
                 /* Protection Register info */
                 extra_size += (extp->NumProtectionFields - 1) *
                               sizeof(struct cfi_intelext_otpinfo);
+        }
 
+        if (extp->MinorVersion >= '1') {
                 /* Burst Read info */
                 extra_size += 2;
                 if (extp_size < sizeof(*extp) + extra_size)
                         goto need_more;
-                extra_size += extp->extra[extra_size-1];
+                extra_size += extp->extra[extra_size - 1];
+        }
+
+        if (extp->MinorVersion >= '3') {
+                int nb_parts, i;
 
                 /* Number of hardware-partitions */
                 extra_size += 1;
diff --git a/drivers/mtd/chips/jedec_probe.c b/drivers/mtd/chips/jedec_probe.c
index e824b9b9b056..ccc4cfc7e4b5 100644
--- a/drivers/mtd/chips/jedec_probe.c
+++ b/drivers/mtd/chips/jedec_probe.c
@@ -166,6 +166,7 @@
 #define SST39LF040      0x00D7
 #define SST39SF010A     0x00B5
 #define SST39SF020A     0x00B6
+#define SST39SF040      0x00B7
 #define SST49LF004B     0x0060
 #define SST49LF040B     0x0050
 #define SST49LF008A     0x005a
@@ -1393,6 +1394,18 @@ static const struct amd_flash_info jedec_table[] = {
                 }
         }, {
                 .mfr_id         = MANUFACTURER_SST,
+                .dev_id         = SST39SF040,
+                .name           = "SST 39SF040",
+                .devtypes       = CFI_DEVICETYPE_X8,
+                .uaddr          = MTD_UADDR_0x5555_0x2AAA,
+                .dev_size       = SIZE_512KiB,
+                .cmd_set        = P_ID_AMD_STD,
+                .nr_regions     = 1,
+                .regions        = {
+                        ERASEINFO(0x01000,128),
+                }
+        }, {
+                .mfr_id         = MANUFACTURER_SST,
                 .dev_id         = SST49LF040B,
                 .name           = "SST 49LF040B",
                 .devtypes       = CFI_DEVICETYPE_X8,
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index cc6369ea67dd..59c46126a5ce 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -500,6 +500,9 @@ static struct flash_info __devinitdata m25p_data [] = {
         { "at26df161a", 0x1f4601, 0, 64 * 1024, 32, SECT_4K, },
         { "at26df321",  0x1f4701, 0, 64 * 1024, 64, SECT_4K, },
 
+        /* Macronix */
+        { "mx25l12805d", 0xc22018, 0, 64 * 1024, 256, },
+
         /* Spansion -- single (large) sector size only, at least
          * for the chips listed here (without boot sectors).
          */
@@ -528,6 +531,7 @@ static struct flash_info __devinitdata m25p_data [] = {
         { "m25p64",  0x202017,  0, 64 * 1024, 128, },
         { "m25p128", 0x202018, 0, 256 * 1024, 64, },
 
+        { "m45pe10", 0x204011,  0, 64 * 1024, 2, },
         { "m45pe80", 0x204014,  0, 64 * 1024, 16, },
         { "m45pe16", 0x204015,  0, 64 * 1024, 32, },
 
diff --git a/drivers/mtd/maps/Kconfig b/drivers/mtd/maps/Kconfig
index 82923bd2d9c5..0b98654d8eed 100644
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -105,15 +105,6 @@ config MSP_FLASH_MAP_LIMIT
         default "0x02000000"
         depends on MSP_FLASH_MAP_LIMIT_32M
 
-config MTD_PMC_MSP_RAMROOT
-        tristate "Embedded RAM block device for root on PMC-Sierra MSP"
-        depends on PMC_MSP_EMBEDDED_ROOTFS && \
-                        (MTD_BLOCK || MTD_BLOCK_RO) && \
-                        MTD_RAM
-        help
-          This provides support for the embedded root file system
-          on PMC MSP devices.  This memory is mapped as a MTD block device.
-
 config MTD_SUN_UFLASH
         tristate "Sun Microsystems userflash support"
         depends on SPARC && MTD_CFI && PCI
@@ -270,7 +261,7 @@ config MTD_ALCHEMY
 
 config MTD_DILNETPC
         tristate "CFI Flash device mapped on DIL/Net PC"
-        depends on X86 && MTD_CONCAT && MTD_PARTITIONS && MTD_CFI_INTELEXT
+        depends on X86 && MTD_CONCAT && MTD_PARTITIONS && MTD_CFI_INTELEXT && BROKEN
         help
           MTD map driver for SSV DIL/Net PC Boards "DNP" and "ADNP".
           For details, see <http://www.ssv-embedded.de/ssv/pc104/p169.htm>
@@ -501,7 +492,7 @@ config MTD_BFIN_ASYNC
           If compiled as a module, it will be called bfin-async-flash.
 
 config MTD_UCLINUX
-        tristate "Generic uClinux RAM/ROM filesystem support"
+        bool "Generic uClinux RAM/ROM filesystem support"
         depends on MTD_PARTITIONS && MTD_RAM && !MMU
         help
           Map driver to support image based filesystems for uClinux.
diff --git a/drivers/mtd/maps/Makefile b/drivers/mtd/maps/Makefile
index 2dbc1bec8488..8bae7f9850c0 100644
--- a/drivers/mtd/maps/Makefile
+++ b/drivers/mtd/maps/Makefile
@@ -25,7 +25,6 @@ obj-$(CONFIG_MTD_OCTAGON)	+= octagon-5066.o
 obj-$(CONFIG_MTD_PHYSMAP)       += physmap.o
 obj-$(CONFIG_MTD_PHYSMAP_OF)    += physmap_of.o
 obj-$(CONFIG_MTD_PMC_MSP_EVM)   += pmcmsp-flash.o
-obj-$(CONFIG_MTD_PMC_MSP_RAMROOT)+= pmcmsp-ramroot.o
 obj-$(CONFIG_MTD_PCMCIA)        += pcmciamtd.o
 obj-$(CONFIG_MTD_RPXLITE)       += rpxlite.o
 obj-$(CONFIG_MTD_TQM8XXL)       += tqm8xxl.o
diff --git a/drivers/mtd/maps/bfin-async-flash.c b/drivers/mtd/maps/bfin-async-flash.c
index 576611f605db..365c77b1b871 100644
--- a/drivers/mtd/maps/bfin-async-flash.c
+++ b/drivers/mtd/maps/bfin-async-flash.c
@@ -40,6 +40,9 @@ struct async_state {
         uint32_t flash_ambctl0, flash_ambctl1;
         uint32_t save_ambctl0, save_ambctl1;
         unsigned long irq_flags;
+#ifdef CONFIG_MTD_PARTITIONS
+        struct mtd_partition *parts;
+#endif
 };
 
 static void switch_to_flash(struct async_state *state)
@@ -170,6 +173,7 @@ static int __devinit bfin_flash_probe(struct platform_device *pdev)
         if (ret > 0) {
                 pr_devinit(KERN_NOTICE DRIVER_NAME ": Using commandline partition definition\n");
                 add_mtd_partitions(state->mtd, pdata->parts, ret);
+                state->parts = pdata->parts;
 
         } else if (pdata->nr_parts) {
                 pr_devinit(KERN_NOTICE DRIVER_NAME ": Using board partition definition\n");
@@ -193,6 +197,7 @@ static int __devexit bfin_flash_remove(struct platform_device *pdev)
         gpio_free(state->enet_flash_pin);
 #ifdef CONFIG_MTD_PARTITIONS
         del_mtd_partitions(state->mtd);
+        kfree(state->parts);
 #endif
         map_destroy(state->mtd);
         kfree(state);
diff --git a/drivers/mtd/maps/integrator-flash.c b/drivers/mtd/maps/integrator-flash.c
index c9681a339a59..b08a798ee254 100644
--- a/drivers/mtd/maps/integrator-flash.c
+++ b/drivers/mtd/maps/integrator-flash.c
@@ -36,27 +36,33 @@
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/map.h>
 #include <linux/mtd/partitions.h>
+#include <linux/mtd/concat.h>
 
 #include <asm/mach/flash.h>
 #include <mach/hardware.h>
 #include <asm/system.h>
 
-#ifdef CONFIG_ARCH_P720T
-#define FLASH_BASE              (0x04000000)
-#define FLASH_SIZE              (64*1024*1024)
-#endif
+#define SUBDEV_NAME_SIZE        (BUS_ID_SIZE + 2)
 
-struct armflash_info {
+struct armflash_subdev_info {
+        char                    name[SUBDEV_NAME_SIZE];
+        struct mtd_info         *mtd;
+        struct map_info         map;
         struct flash_platform_data *plat;
+};
+
+struct armflash_info {
         struct resource         *res;
         struct mtd_partition    *parts;
         struct mtd_info         *mtd;
-        struct map_info         map;
+        int                     nr_subdev;
+        struct armflash_subdev_info subdev[0];
 };
 
 static void armflash_set_vpp(struct map_info *map, int on)
 {
-        struct armflash_info *info = container_of(map, struct armflash_info, map);
+        struct armflash_subdev_info *info =
+                container_of(map, struct armflash_subdev_info, map);
 
         if (info->plat && info->plat->set_vpp)
                 info->plat->set_vpp(on);
@@ -64,32 +70,17 @@ static void armflash_set_vpp(struct map_info *map, int on)
 
 static const char *probes[] = { "cmdlinepart", "RedBoot", "afs", NULL };
 
-static int armflash_probe(struct platform_device *dev)
+static int armflash_subdev_probe(struct armflash_subdev_info *subdev,
+                                 struct resource *res)
 {
-        struct flash_platform_data *plat = dev->dev.platform_data;
-        struct resource *res = dev->resource;
-        unsigned int size = res->end - res->start + 1;
-        struct armflash_info *info;
-        int err;
+        struct flash_platform_data *plat = subdev->plat;
+        resource_size_t size = res->end - res->start + 1;
         void __iomem *base;
+        int err = 0;
 
-        info = kzalloc(sizeof(struct armflash_info), GFP_KERNEL);
-        if (!info) {
-                err = -ENOMEM;
-                goto out;
-        }
-
-        info->plat = plat;
-        if (plat && plat->init) {
-                err = plat->init();
-                if (err)
-                        goto no_resource;
-        }
-
-        info->res = request_mem_region(res->start, size, "armflash");
-        if (!info->res) {
+        if (!request_mem_region(res->start, size, subdev->name)) {
                 err = -EBUSY;
-                goto no_resource;
+                goto out;
         }
 
         base = ioremap(res->start, size);
@@ -101,27 +92,132 @@ static int armflash_probe(struct platform_device *dev)
         /*
          * look for CFI based flash parts fitted to this board
          */
-        info->map.size          = size;
-        info->map.bankwidth     = plat->width;
-        info->map.phys          = res->start;
-        info->map.virt          = base;
-        info->map.name          = dev_name(&dev->dev);
-        info->map.set_vpp       = armflash_set_vpp;
+        subdev->map.size        = size;
+        subdev->map.bankwidth   = plat->width;
+        subdev->map.phys        = res->start;
+        subdev->map.virt        = base;
+        subdev->map.name        = subdev->name;
+        subdev->map.set_vpp     = armflash_set_vpp;
 
-        simple_map_init(&info->map);
+        simple_map_init(&subdev->map);
 
         /*
          * Also, the CFI layer automatically works out what size
          * of chips we have, and does the necessary identification
          * for us automatically.
          */
-        info->mtd = do_map_probe(plat->map_name, &info->map);
-        if (!info->mtd) {
+        subdev->mtd = do_map_probe(plat->map_name, &subdev->map);
+        if (!subdev->mtd) {
                 err = -ENXIO;
                 goto no_device;
         }
 
-        info->mtd->owner = THIS_MODULE;
+        subdev->mtd->owner = THIS_MODULE;
+
+        /* Successful? */
+        if (err == 0)
+                return err;
+
+        if (subdev->mtd)
+                map_destroy(subdev->mtd);
+ no_device:
+        iounmap(base);
+ no_mem:
+        release_mem_region(res->start, size);
+ out:
+        return err;
+}
+
+static void armflash_subdev_remove(struct armflash_subdev_info *subdev)
+{
+        if (subdev->mtd)
+                map_destroy(subdev->mtd);
+        if (subdev->map.virt)
+                iounmap(subdev->map.virt);
+        release_mem_region(subdev->map.phys, subdev->map.size);
+}
+
+static int armflash_probe(struct platform_device *dev)
+{
+        struct flash_platform_data *plat = dev->dev.platform_data;
+        unsigned int size;
+        struct armflash_info *info;
+        int i, nr, err;
+
+        /* Count the number of devices */
+        for (nr = 0; ; nr++)
+                if (!platform_get_resource(dev, IORESOURCE_MEM, nr))
+                        break;
+        if (nr == 0) {
+                err = -ENODEV;
+                goto out;
+        }
+
+        size = sizeof(struct armflash_info) +
+                sizeof(struct armflash_subdev_info) * nr;
+        info = kzalloc(size, GFP_KERNEL);
+        if (!info) {
+                err = -ENOMEM;
+                goto out;
+        }
+
+        if (plat && plat->init) {
+                err = plat->init();
+                if (err)
+                        goto no_resource;
+        }
+
+        for (i = 0; i < nr; i++) {
+                struct armflash_subdev_info *subdev = &info->subdev[i];
+                struct resource *res;
+
+                res = platform_get_resource(dev, IORESOURCE_MEM, i);
+                if (!res)
+                        break;
+
+                if (nr == 1)
+                        /* No MTD concatenation, just use the default name */
+                        snprintf(subdev->name, SUBDEV_NAME_SIZE, "%s",
+                                 dev_name(&dev->dev));
+                else
+                        snprintf(subdev->name, SUBDEV_NAME_SIZE, "%s-%d",
+                                 dev_name(&dev->dev), i);
+                subdev->plat = plat;
+
+                err = armflash_subdev_probe(subdev, res);
+                if (err)
+                        break;
+        }
+        info->nr_subdev = i;
+
+        if (err)
+                goto subdev_err;
+
+        if (info->nr_subdev == 1)
+                info->mtd = info->subdev[0].mtd;
+        else if (info->nr_subdev > 1) {
+#ifdef CONFIG_MTD_CONCAT
+                struct mtd_info *cdev[info->nr_subdev];
+
+                /*
+                 * We detected multiple devices.  Concatenate them together.
+                 */
+                for (i = 0; i < info->nr_subdev; i++)
+                        cdev[i] = info->subdev[i].mtd;
+
+                info->mtd = mtd_concat_create(cdev, info->nr_subdev,
+                                              dev_name(&dev->dev));
+                if (info->mtd == NULL)
+                        err = -ENXIO;
+#else
+                printk(KERN_ERR "armflash: multiple devices found but "
+                       "MTD concat support disabled.\n");
+                err = -ENXIO;
+#endif
+        }
+
+        if (err < 0)
+                goto cleanup;
 
         err = parse_mtd_partitions(info->mtd, probes, &info->parts, 0);
         if (err > 0) {
@@ -131,28 +227,30 @@ static int armflash_probe(struct platform_device *dev)
                                "mtd partition registration failed: %d\n", err);
         }
 
-        if (err == 0)
+        if (err == 0) {
                 platform_set_drvdata(dev, info);
+                return err;
+        }
 
         /*
-         * If we got an error, free all resources.
+         * We got an error, free all resources.
          */
-        if (err < 0) {
-                if (info->mtd) {
-                        del_mtd_partitions(info->mtd);
-                        map_destroy(info->mtd);
-                }
-                kfree(info->parts);
-
- no_device:
-                iounmap(base);
- no_mem:
-                release_mem_region(res->start, size);
- no_resource:
-                if (plat && plat->exit)
-                        plat->exit();
-                kfree(info);
+ cleanup:
+        if (info->mtd) {
+                del_mtd_partitions(info->mtd);
+#ifdef CONFIG_MTD_CONCAT
+                if (info->mtd != info->subdev[0].mtd)
+                        mtd_concat_destroy(info->mtd);
+#endif
         }
+        kfree(info->parts);
+ subdev_err:
+        for (i = info->nr_subdev - 1; i >= 0; i--)
+                armflash_subdev_remove(&info->subdev[i]);
+ no_resource:
+        if (plat && plat->exit)
+                plat->exit();
+        kfree(info);
  out:
         return err;
 }
@@ -160,22 +258,26 @@ static int armflash_probe(struct platform_device *dev)
 static int armflash_remove(struct platform_device *dev)
 {
         struct armflash_info *info = platform_get_drvdata(dev);
+        struct flash_platform_data *plat = dev->dev.platform_data;
+        int i;
 
         platform_set_drvdata(dev, NULL);
 
         if (info) {
                 if (info->mtd) {
                         del_mtd_partitions(info->mtd);
-                        map_destroy(info->mtd);
+#ifdef CONFIG_MTD_CONCAT
+                        if (info->mtd != info->subdev[0].mtd)
+                                mtd_concat_destroy(info->mtd);
+#endif
                 }
                 kfree(info->parts);
 
-                iounmap(info->map.virt);
-                release_resource(info->res);
-                kfree(info->res);
+                for (i = info->nr_subdev - 1; i >= 0; i--)
+                        armflash_subdev_remove(&info->subdev[i]);
 
-                if (info->plat && info->plat->exit)
-                        info->plat->exit();
+                if (plat && plat->exit)
+                        plat->exit();
 
                 kfree(info);
         }
diff --git a/drivers/mtd/maps/physmap.c b/drivers/mtd/maps/physmap.c
index 29a901157352..380648e9051a 100644
--- a/drivers/mtd/maps/physmap.c
+++ b/drivers/mtd/maps/physmap.c
@@ -195,42 +195,6 @@ err_out:
 }
 
 #ifdef CONFIG_PM
-static int physmap_flash_suspend(struct platform_device *dev, pm_message_t state)
-{
-        struct physmap_flash_info *info = platform_get_drvdata(dev);
-        int ret = 0;
-        int i;
-
-        for (i = 0; i < MAX_RESOURCES && info->mtd[i]; i++)
-                if (info->mtd[i]->suspend) {
-                        ret = info->mtd[i]->suspend(info->mtd[i]);
-                        if (ret)
-                                goto fail;
-                }
-
-        return 0;
-fail:
-        for (--i; i >= 0; --i)
-                if (info->mtd[i]->suspend) {
-                        BUG_ON(!info->mtd[i]->resume);
-                        info->mtd[i]->resume(info->mtd[i]);
-                }
-
-        return ret;
-}
-
-static int physmap_flash_resume(struct platform_device *dev)
-{
-        struct physmap_flash_info *info = platform_get_drvdata(dev);
-        int i;
-
-        for (i = 0; i < MAX_RESOURCES && info->mtd[i]; i++)
-                if (info->mtd[i]->resume)
-                        info->mtd[i]->resume(info->mtd[i]);
-
-        return 0;
-}
-
 static void physmap_flash_shutdown(struct platform_device *dev)
 {
         struct physmap_flash_info *info = platform_get_drvdata(dev);
@@ -242,16 +206,12 @@ static void physmap_flash_shutdown(struct platform_device *dev)
                                 info->mtd[i]->resume(info->mtd[i]);
 }
 #else
-#define physmap_flash_suspend NULL
-#define physmap_flash_resume NULL
 #define physmap_flash_shutdown NULL
 #endif
 
 static struct platform_driver physmap_flash_driver = {
         .probe          = physmap_flash_probe,
         .remove         = physmap_flash_remove,
-        .suspend        = physmap_flash_suspend,
-        .resume         = physmap_flash_resume,
         .shutdown       = physmap_flash_shutdown,
         .driver         = {
                 .name   = "physmap-flash",
diff --git a/drivers/mtd/maps/physmap_of.c b/drivers/mtd/maps/physmap_of.c
index c83a60fada53..39d357b2eb47 100644
--- a/drivers/mtd/maps/physmap_of.c
+++ b/drivers/mtd/maps/physmap_of.c
@@ -20,16 +20,23 @@
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/map.h>
 #include <linux/mtd/partitions.h>
+#include <linux/mtd/concat.h>
 #include <linux/of.h>
 #include <linux/of_platform.h>
 
+struct of_flash_list {
+        struct mtd_info *mtd;
+        struct map_info map;
+        struct resource *res;
+};
+
 struct of_flash {
-        struct mtd_info         *mtd;
-        struct map_info         map;
-        struct resource         *res;
+        struct mtd_info         *cmtd;
 #ifdef CONFIG_MTD_PARTITIONS
         struct mtd_partition    *parts;
 #endif
+        int list_size; /* number of elements in of_flash_list */
+        struct of_flash_list    list[0];
 };
 
 #ifdef CONFIG_MTD_PARTITIONS
@@ -88,30 +95,44 @@ static int parse_obsolete_partitions(struct of_device *dev,
 static int of_flash_remove(struct of_device *dev)
 {
         struct of_flash *info;
+        int i;
 
         info = dev_get_drvdata(&dev->dev);
         if (!info)
                 return 0;
         dev_set_drvdata(&dev->dev, NULL);
 
-        if (info->mtd) {
+#ifdef CONFIG_MTD_CONCAT
+        if (info->cmtd != info->list[0].mtd) {
+                del_mtd_device(info->cmtd);
+                mtd_concat_destroy(info->cmtd);
+        }
+#endif
+
+        if (info->cmtd) {
                 if (OF_FLASH_PARTS(info)) {
-                        del_mtd_partitions(info->mtd);
+                        del_mtd_partitions(info->cmtd);
                         kfree(OF_FLASH_PARTS(info));
                 } else {
-                        del_mtd_device(info->mtd);
+                        del_mtd_device(info->cmtd);
                 }
-                map_destroy(info->mtd);
         }
 
-        if (info->map.virt)
-                iounmap(info->map.virt);
+        for (i = 0; i < info->list_size; i++) {
+                if (info->list[i].mtd)
+                        map_destroy(info->list[i].mtd);
 
-        if (info->res) {
-                release_resource(info->res);
-                kfree(info->res);
+                if (info->list[i].map.virt)
+                        iounmap(info->list[i].map.virt);
+
+                if (info->list[i].res) {
+                        release_resource(info->list[i].res);
+                        kfree(info->list[i].res);
+                }
         }
 
+        kfree(info);
+
         return 0;
 }
 
@@ -164,68 +185,130 @@ static int __devinit of_flash_probe(struct of_device *dev,
         const char *probe_type = match->data;
         const u32 *width;
         int err;
-
-        err = -ENXIO;
-        if (of_address_to_resource(dp, 0, &res)) {
-                dev_err(&dev->dev, "Can't get IO address from device tree\n");
+        int i;
+        int count;
+        const u32 *p;
+        int reg_tuple_size;
+        struct mtd_info **mtd_list = NULL;
+
+        reg_tuple_size = (of_n_addr_cells(dp) + of_n_size_cells(dp)) * sizeof(u32);
+
+        /*
+         * Get number of "reg" tuples. Scan for MTD devices on area's
+         * described by each "reg" region. This makes it possible (including
+         * the concat support) to support the Intel P30 48F4400 chips which
+         * consists internally of 2 non-identical NOR chips on one die.
+         */
+        p = of_get_property(dp, "reg", &count);
+        if (count % reg_tuple_size != 0) {
+                dev_err(&dev->dev, "Malformed reg property on %s\n",
+                                dev->node->full_name);
+                err = -EINVAL;
                 goto err_out;
         }
-
-        dev_dbg(&dev->dev, "of_flash device: %.8llx-%.8llx\n",
-                (unsigned long long)res.start, (unsigned long long)res.end);
+        count /= reg_tuple_size;
 
         err = -ENOMEM;
-        info = kzalloc(sizeof(*info), GFP_KERNEL);
+        info = kzalloc(sizeof(struct of_flash) +
+                       sizeof(struct of_flash_list) * count, GFP_KERNEL);
+        if (!info)
+                goto err_out;
+
+        mtd_list = kzalloc(sizeof(struct mtd_info) * count, GFP_KERNEL);
         if (!info)
                 goto err_out;
 
         dev_set_drvdata(&dev->dev, info);
 
-        err = -EBUSY;
-        info->res = request_mem_region(res.start, res.end - res.start + 1,
-                                       dev_name(&dev->dev));
-        if (!info->res)
-                goto err_out;
+        for (i = 0; i < count; i++) {
+                err = -ENXIO;
+                if (of_address_to_resource(dp, i, &res)) {
+                        dev_err(&dev->dev, "Can't get IO address from device"
+                                " tree\n");
+                        goto err_out;
+                }
 
-        err = -ENXIO;
-        width = of_get_property(dp, "bank-width", NULL);
-        if (!width) {
-                dev_err(&dev->dev, "Can't get bank width from device tree\n");
-                goto err_out;
-        }
+                dev_dbg(&dev->dev, "of_flash device: %.8llx-%.8llx\n",
+                        (unsigned long long)res.start,
+                        (unsigned long long)res.end);
+
+                err = -EBUSY;
+                info->list[i].res = request_mem_region(res.start, res.end -
+                                                       res.start + 1,
+                                                       dev_name(&dev->dev));
+                if (!info->list[i].res)
+                        goto err_out;
+
+                err = -ENXIO;
+                width = of_get_property(dp, "bank-width", NULL);
+                if (!width) {
+                        dev_err(&dev->dev, "Can't get bank width from device"
+                                " tree\n");
+                        goto err_out;
+                }
 
-        info->map.name = dev_name(&dev->dev);
-        info->map.phys = res.start;
-        info->map.size = res.end - res.start + 1;
-        info->map.bankwidth = *width;
+                info->list[i].map.name = dev_name(&dev->dev);
+                info->list[i].map.phys = res.start;
+                info->list[i].map.size = res.end - res.start + 1;
+                info->list[i].map.bankwidth = *width;
+
+                err = -ENOMEM;
+                info->list[i].map.virt = ioremap(info->list[i].map.phys,
+                                                 info->list[i].map.size);
+                if (!info->list[i].map.virt) {
+                        dev_err(&dev->dev, "Failed to ioremap() flash"
+                                " region\n");
+                        goto err_out;
+                }
 
-        err = -ENOMEM;
-        info->map.virt = ioremap(info->map.phys, info->map.size);
-        if (!info->map.virt) {
-                dev_err(&dev->dev, "Failed to ioremap() flash region\n");
-                goto err_out;
-        }
+                simple_map_init(&info->list[i].map);
 
-        simple_map_init(&info->map);
+                if (probe_type) {
+                        info->list[i].mtd = do_map_probe(probe_type,
+                                                         &info->list[i].map);
+                } else {
+                        info->list[i].mtd = obsolete_probe(dev,
+                                                           &info->list[i].map);
+                }
+                mtd_list[i] = info->list[i].mtd;
 
-        if (probe_type)
-                info->mtd = do_map_probe(probe_type, &info->map);
-        else
-                info->mtd = obsolete_probe(dev, &info->map);
+                err = -ENXIO;
+                if (!info->list[i].mtd) {
+                        dev_err(&dev->dev, "do_map_probe() failed\n");
+                        goto err_out;
+                } else {
+                        info->list_size++;
+                }
+                info->list[i].mtd->owner = THIS_MODULE;
+                info->list[i].mtd->dev.parent = &dev->dev;
+        }
 
-        err = -ENXIO;
-        if (!info->mtd) {
-                dev_err(&dev->dev, "do_map_probe() failed\n");
-                goto err_out;
+        err = 0;
+        if (info->list_size == 1) {
+                info->cmtd = info->list[0].mtd;
+        } else if (info->list_size > 1) {
+                /*
+                 * We detected multiple devices. Concatenate them together.
+                 */
+#ifdef CONFIG_MTD_CONCAT
+                info->cmtd = mtd_concat_create(mtd_list, info->list_size,
+                                               dev_name(&dev->dev));
+                if (info->cmtd == NULL)
+                        err = -ENXIO;
+#else
+                printk(KERN_ERR "physmap_of: multiple devices "
+                       "found but MTD concat support disabled.\n");
+                err = -ENXIO;
+#endif
         }
-        info->mtd->owner = THIS_MODULE;
-        info->mtd->dev.parent = &dev->dev;
+        if (err)
+                goto err_out;
 
 #ifdef CONFIG_MTD_PARTITIONS
         /* First look for RedBoot table or partitions on the command
          * line, these take precedence over device tree information */
-        err = parse_mtd_partitions(info->mtd, part_probe_types,
-                                   &info->parts, 0);
+        err = parse_mtd_partitions(info->cmtd, part_probe_types,
+                                   &info->parts, 0);
         if (err < 0)
                 return err;
 
@@ -244,15 +327,19 @@ static int __devinit of_flash_probe(struct of_device *dev,
         }
 
         if (err > 0)
-                add_mtd_partitions(info->mtd, info->parts, err);
+                add_mtd_partitions(info->cmtd, info->parts, err);
         else
 #endif
-                add_mtd_device(info->mtd);
+                add_mtd_device(info->cmtd);
+
+        kfree(mtd_list);
 
         return 0;
 
 err_out:
+        kfree(mtd_list);
         of_flash_remove(dev);
+
         return err;
 }
 
diff --git a/drivers/mtd/maps/pmcmsp-ramroot.c b/drivers/mtd/maps/pmcmsp-ramroot.c
deleted file mode 100644
index 30de5c0c09a9..000000000000
--- a/drivers/mtd/maps/pmcmsp-ramroot.c
+++ /dev/null
@@ -1,104 +0,0 @@
-/*
- * Mapping of the rootfs in a physical region of memory
- *
- * Copyright (C) 2005-2007 PMC-Sierra Inc.
- * Author: Andrew Hughes, Andrew_Hughes@pmc-sierra.com
- *
- *  This program is free software; you can redistribute  it and/or modify it
- *  under  the terms of  the GNU General  Public License as published by the
- *  Free Software Foundation;  either version 2 of the  License, or (at your
- *  option) any later version.
- *
- *  THIS  SOFTWARE  IS PROVIDED   ``AS  IS'' AND   ANY  EXPRESS OR IMPLIED
- *  WARRANTIES,   INCLUDING, BUT NOT  LIMITED  TO, THE IMPLIED WARRANTIES OF
- *  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN
- *  NO  EVENT  SHALL   THE AUTHOR  BE    LIABLE FOR ANY   DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
- *  NOT LIMITED   TO, PROCUREMENT OF  SUBSTITUTE GOODS  OR SERVICES; LOSS OF
- *  USE, DATA,  OR PROFITS; OR  BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
- *  ANY THEORY OF LIABILITY, WHETHER IN  CONTRACT, STRICT LIABILITY, OR TORT
- *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
- *  THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- *  You should have received a copy of the  GNU General Public License along
- *  with this program; if not, write  to the Free Software Foundation, Inc.,
- *  675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#include <linux/module.h>
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/fs.h>
-#include <linux/root_dev.h>
-#include <linux/mtd/mtd.h>
-#include <linux/mtd/map.h>
-
-#include <asm/io.h>
-
-#include <msp_prom.h>
-
-static struct mtd_info *rr_mtd;
-
-struct map_info rr_map = {
-        .name = "ramroot",
-        .bankwidth = 4,
-};
-
-static int __init init_rrmap(void)
-{
-        void *ramroot_start;
-        unsigned long ramroot_size;
-
-        /* Check for supported rootfs types */
-        if (get_ramroot(&ramroot_start, &ramroot_size)) {
-                rr_map.phys = CPHYSADDR(ramroot_start);
-                rr_map.size = ramroot_size;
-
-                printk(KERN_NOTICE
-                        "PMC embedded root device: 0x%08lx @ 0x%08lx\n",
-                        rr_map.size, (unsigned long)rr_map.phys);
-        } else {
-                printk(KERN_ERR
-                        "init_rrmap: no supported embedded rootfs detected!\n");
-                return -ENXIO;
-        }
-
-        /* Map rootfs to I/O space for block device driver */
-        rr_map.virt = ioremap(rr_map.phys, rr_map.size);
-        if (!rr_map.virt) {
-                printk(KERN_ERR "Failed to ioremap\n");
-                return -EIO;
-        }
-
-        simple_map_init(&rr_map);
-
-        rr_mtd = do_map_probe("map_ram", &rr_map);
-        if (rr_mtd) {
-                rr_mtd->owner = THIS_MODULE;
-
-                add_mtd_device(rr_mtd);
-
-                return 0;
-        }
-
-        iounmap(rr_map.virt);
-        return -ENXIO;
-}
-
-static void __exit cleanup_rrmap(void)
-{
-        del_mtd_device(rr_mtd);
-        map_destroy(rr_mtd);
-
-        iounmap(rr_map.virt);
-        rr_map.virt = NULL;
-}
-
-MODULE_AUTHOR("PMC-Sierra, Inc");
-MODULE_DESCRIPTION("MTD map driver for embedded PMC-Sierra MSP filesystem");
-MODULE_LICENSE("GPL");
-
-module_init(init_rrmap);
-module_exit(cleanup_rrmap);
diff --git a/drivers/mtd/maps/pxa2xx-flash.c b/drivers/mtd/maps/pxa2xx-flash.c
index 572d32fdf38a..643aa06b599e 100644
--- a/drivers/mtd/maps/pxa2xx-flash.c
+++ b/drivers/mtd/maps/pxa2xx-flash.c
@@ -140,24 +140,6 @@ static int __devexit pxa2xx_flash_remove(struct platform_device *dev)
 }
 
 #ifdef CONFIG_PM
-static int pxa2xx_flash_suspend(struct platform_device *dev, pm_message_t state)
-{
-        struct pxa2xx_flash_info *info = platform_get_drvdata(dev);
-        int ret = 0;
-
-        if (info->mtd && info->mtd->suspend)
-                ret = info->mtd->suspend(info->mtd);
-        return ret;
-}
-
-static int pxa2xx_flash_resume(struct platform_device *dev)
-{
-        struct pxa2xx_flash_info *info = platform_get_drvdata(dev);
-
-        if (info->mtd && info->mtd->resume)
-                info->mtd->resume(info->mtd);
-        return 0;
-}
 static void pxa2xx_flash_shutdown(struct platform_device *dev)
 {
         struct pxa2xx_flash_info *info = platform_get_drvdata(dev);
@@ -166,8 +148,6 @@ static void pxa2xx_flash_shutdown(struct platform_device *dev)
                 info->mtd->resume(info->mtd);
 }
 #else
-#define pxa2xx_flash_suspend NULL
-#define pxa2xx_flash_resume NULL
 #define pxa2xx_flash_shutdown NULL
 #endif
 
@@ -178,8 +158,6 @@ static struct platform_driver pxa2xx_flash_driver = {
         },
         .probe          = pxa2xx_flash_probe,
         .remove         = __devexit_p(pxa2xx_flash_remove),
-        .suspend        = pxa2xx_flash_suspend,
-        .resume         = pxa2xx_flash_resume,
         .shutdown       = pxa2xx_flash_shutdown,
 };
 
diff --git a/drivers/mtd/maps/rbtx4939-flash.c b/drivers/mtd/maps/rbtx4939-flash.c
index d39f0adac846..83ed64512c5e 100644
--- a/drivers/mtd/maps/rbtx4939-flash.c
+++ b/drivers/mtd/maps/rbtx4939-flash.c
@@ -145,25 +145,6 @@ err_out:
 }
 
 #ifdef CONFIG_PM
-static int rbtx4939_flash_suspend(struct platform_device *dev,
-                                  pm_message_t state)
-{
-        struct rbtx4939_flash_info *info = platform_get_drvdata(dev);
-
-        if (info->mtd->suspend)
-                return info->mtd->suspend(info->mtd);
-        return 0;
-}
-
-static int rbtx4939_flash_resume(struct platform_device *dev)
-{
-        struct rbtx4939_flash_info *info = platform_get_drvdata(dev);
-
-        if (info->mtd->resume)
-                info->mtd->resume(info->mtd);
-        return 0;
-}
-
 static void rbtx4939_flash_shutdown(struct platform_device *dev)
 {
         struct rbtx4939_flash_info *info = platform_get_drvdata(dev);
@@ -173,16 +154,12 @@ static void rbtx4939_flash_shutdown(struct platform_device *dev)
                         info->mtd->resume(info->mtd);
 }
 #else
-#define rbtx4939_flash_suspend NULL
-#define rbtx4939_flash_resume NULL
 #define rbtx4939_flash_shutdown NULL
 #endif
 
 static struct platform_driver rbtx4939_flash_driver = {
         .probe          = rbtx4939_flash_probe,
         .remove         = rbtx4939_flash_remove,
-        .suspend        = rbtx4939_flash_suspend,
-        .resume         = rbtx4939_flash_resume,
         .shutdown       = rbtx4939_flash_shutdown,
         .driver         = {
                 .name   = "rbtx4939-flash",
diff --git a/drivers/mtd/maps/sa1100-flash.c b/drivers/mtd/maps/sa1100-flash.c
index 05e9362dc7f0..c6210f5118d1 100644
--- a/drivers/mtd/maps/sa1100-flash.c
+++ b/drivers/mtd/maps/sa1100-flash.c
@@ -415,25 +415,6 @@ static int __exit sa1100_mtd_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_PM
-static int sa1100_mtd_suspend(struct platform_device *dev, pm_message_t state)
-{
-        struct sa_info *info = platform_get_drvdata(dev);
-        int ret = 0;
-
-        if (info)
-                ret = info->mtd->suspend(info->mtd);
-
-        return ret;
-}
-
-static int sa1100_mtd_resume(struct platform_device *dev)
-{
-        struct sa_info *info = platform_get_drvdata(dev);
-        if (info)
-                info->mtd->resume(info->mtd);
-        return 0;
-}
-
 static void sa1100_mtd_shutdown(struct platform_device *dev)
 {
         struct sa_info *info = platform_get_drvdata(dev);
@@ -441,16 +422,12 @@ static void sa1100_mtd_shutdown(struct platform_device *dev)
                 info->mtd->resume(info->mtd);
 }
 #else
-#define sa1100_mtd_suspend NULL
-#define sa1100_mtd_resume  NULL
 #define sa1100_mtd_shutdown NULL
 #endif
 
 static struct platform_driver sa1100_mtd_driver = {
         .probe          = sa1100_mtd_probe,
         .remove         = __exit_p(sa1100_mtd_remove),
-        .suspend        = sa1100_mtd_suspend,
-        .resume         = sa1100_mtd_resume,
         .shutdown       = sa1100_mtd_shutdown,
         .driver         = {
                 .name   = "sa1100-mtd",
diff --git a/drivers/mtd/maps/uclinux.c b/drivers/mtd/maps/uclinux.c
index 81756e397711..d4314fb88212 100644
--- a/drivers/mtd/maps/uclinux.c
+++ b/drivers/mtd/maps/uclinux.c
@@ -22,15 +22,19 @@
 
 /****************************************************************************/
 
+extern char _ebss;
+
 struct map_info uclinux_ram_map = {
         .name = "RAM",
+        .phys = (unsigned long)&_ebss,
+        .size = 0,
 };
 
-struct mtd_info *uclinux_ram_mtdinfo;
+static struct mtd_info *uclinux_ram_mtdinfo;
 
 /****************************************************************************/
 
-struct mtd_partition uclinux_romfs[] = {
+static struct mtd_partition uclinux_romfs[] = {
         { .name = "ROMfs" }
 };
 
@@ -38,7 +42,7 @@ struct mtd_partition uclinux_romfs[] = {
 
 /****************************************************************************/
 
-int uclinux_point(struct mtd_info *mtd, loff_t from, size_t len,
+static int uclinux_point(struct mtd_info *mtd, loff_t from, size_t len,
         size_t *retlen, void **virt, resource_size_t *phys)
 {
         struct map_info *map = mtd->priv;
@@ -55,12 +59,10 @@ static int __init uclinux_mtd_init(void)
 {
         struct mtd_info *mtd;
         struct map_info *mapp;
-        extern char _ebss;
-        unsigned long addr = (unsigned long) &_ebss;
 
         mapp = &uclinux_ram_map;
-        mapp->phys = addr;
-        mapp->size = PAGE_ALIGN(ntohl(*((unsigned long *)(addr + 8))));
+        if (!mapp->size)
+                mapp->size = PAGE_ALIGN(ntohl(*((unsigned long *)(mapp->phys + 8))));
         mapp->bankwidth = 4;
 
         printk("uclinux[mtd]: RAM probe address=0x%x size=0x%x\n",
diff --git a/drivers/mtd/mtd_blkdevs.c b/drivers/mtd/mtd_blkdevs.c
index aaac3b6800b7..c3f62654b6df 100644
--- a/drivers/mtd/mtd_blkdevs.c
+++ b/drivers/mtd/mtd_blkdevs.c
@@ -291,7 +291,7 @@ int add_mtd_blktrans_dev(struct mtd_blktrans_dev *new)
         gd->private_data = new;
         new->blkcore_priv = gd;
         gd->queue = tr->blkcore_priv->rq;
-        gd->driverfs_dev = new->mtd->dev.parent;
+        gd->driverfs_dev = &new->mtd->dev;
 
         if (new->readonly)
                 set_disk_ro(gd, 1);
diff --git a/drivers/mtd/mtdchar.c b/drivers/mtd/mtdchar.c
index 763d3f0a1f42..5b081cb84351 100644
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -14,6 +14,7 @@
 #include <linux/sched.h>
 #include <linux/smp_lock.h>
 #include <linux/backing-dev.h>
+#include <linux/compat.h>
 
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/compatmac.h>
@@ -355,6 +356,100 @@ static int otp_select_filemode(struct mtd_file_info *mfi, int mode)
 # define otp_select_filemode(f,m)       -EOPNOTSUPP
 #endif
 
+static int mtd_do_writeoob(struct file *file, struct mtd_info *mtd,
+        uint64_t start, uint32_t length, void __user *ptr,
+        uint32_t __user *retp)
+{
+        struct mtd_oob_ops ops;
+        uint32_t retlen;
+        int ret = 0;
+
+        if (!(file->f_mode & FMODE_WRITE))
+                return -EPERM;
+
+        if (length > 4096)
+                return -EINVAL;
+
+        if (!mtd->write_oob)
+                ret = -EOPNOTSUPP;
+        else
+                ret = access_ok(VERIFY_READ, ptr, length) ? 0 : EFAULT;
+
+        if (ret)
+                return ret;
+
+        ops.ooblen = length;
+        ops.ooboffs = start & (mtd->oobsize - 1);
+        ops.datbuf = NULL;
+        ops.mode = MTD_OOB_PLACE;
+
+        if (ops.ooboffs && ops.ooblen > (mtd->oobsize - ops.ooboffs))
+                return -EINVAL;
+
+        ops.oobbuf = kmalloc(length, GFP_KERNEL);
+        if (!ops.oobbuf)
+                return -ENOMEM;
+
+        if (copy_from_user(ops.oobbuf, ptr, length)) {
+                kfree(ops.oobbuf);
+                return -EFAULT;
+        }
+
+        start &= ~((uint64_t)mtd->oobsize - 1);
+        ret = mtd->write_oob(mtd, start, &ops);
+
+        if (ops.oobretlen > 0xFFFFFFFFU)
+                ret = -EOVERFLOW;
+        retlen = ops.oobretlen;
+        if (copy_to_user(retp, &retlen, sizeof(length)))
+                ret = -EFAULT;
+
+        kfree(ops.oobbuf);
+        return ret;
+}
+
+static int mtd_do_readoob(struct mtd_info *mtd, uint64_t start,
+        uint32_t length, void __user *ptr, uint32_t __user *retp)
+{
+        struct mtd_oob_ops ops;
+        int ret = 0;
+
+        if (length > 4096)
+                return -EINVAL;
+
+        if (!mtd->read_oob)
+                ret = -EOPNOTSUPP;
+        else
+                ret = access_ok(VERIFY_WRITE, ptr,
+                                length) ? 0 : -EFAULT;
+        if (ret)
+                return ret;
+
+        ops.ooblen = length;
+        ops.ooboffs = start & (mtd->oobsize - 1);
+        ops.datbuf = NULL;
+        ops.mode = MTD_OOB_PLACE;
+
+        if (ops.ooboffs && ops.ooblen > (mtd->oobsize - ops.ooboffs))
+                return -EINVAL;
+
+        ops.oobbuf = kmalloc(length, GFP_KERNEL);
+        if (!ops.oobbuf)
+                return -ENOMEM;
+
+        start &= ~((uint64_t)mtd->oobsize - 1);
+        ret = mtd->read_oob(mtd, start, &ops);
+
+        if (put_user(ops.oobretlen, retp))
+                ret = -EFAULT;
+        else if (ops.oobretlen && copy_to_user(ptr, ops.oobbuf,
+                                            ops.oobretlen))
+                ret = -EFAULT;
+
+        kfree(ops.oobbuf);
+        return ret;
+}
+
 static int mtd_ioctl(struct inode *inode, struct file *file,
                      u_int cmd, u_long arg)
 {
@@ -417,6 +512,7 @@ static int mtd_ioctl(struct inode *inode, struct file *file,
                 break;
 
         case MEMERASE:
+        case MEMERASE64:
         {
                 struct erase_info *erase;
 
@@ -427,20 +523,32 @@ static int mtd_ioctl(struct inode *inode, struct file *file,
                 if (!erase)
                         ret = -ENOMEM;
                 else {
-                        struct erase_info_user einfo;
-
                         wait_queue_head_t waitq;
                         DECLARE_WAITQUEUE(wait, current);
 
                         init_waitqueue_head(&waitq);
 
-                        if (copy_from_user(&einfo, argp,
-                                    sizeof(struct erase_info_user))) {
-                                kfree(erase);
-                                return -EFAULT;
+                        if (cmd == MEMERASE64) {
+                                struct erase_info_user64 einfo64;
+
+                                if (copy_from_user(&einfo64, argp,
+                                            sizeof(struct erase_info_user64))) {
+                                        kfree(erase);
+                                        return -EFAULT;
+                                }
+                                erase->addr = einfo64.start;
+                                erase->len = einfo64.length;
+                        } else {
+                                struct erase_info_user einfo32;
+
+                                if (copy_from_user(&einfo32, argp,
+                                            sizeof(struct erase_info_user))) {
+                                        kfree(erase);
+                                        return -EFAULT;
+                                }
+                                erase->addr = einfo32.start;
+                                erase->len = einfo32.length;
                         }
-                        erase->addr = einfo.start;
-                        erase->len = einfo.length;
                         erase->mtd = mtd;
                         erase->callback = mtdchar_erase_callback;
                         erase->priv = (unsigned long)&waitq;
@@ -474,100 +582,56 @@ static int mtd_ioctl(struct inode *inode, struct file *file,
         case MEMWRITEOOB:
         {
                 struct mtd_oob_buf buf;
-                struct mtd_oob_ops ops;
-                struct mtd_oob_buf __user *user_buf = argp;
-                uint32_t retlen;
-
-                if(!(file->f_mode & FMODE_WRITE))
-                        return -EPERM;
-
-                if (copy_from_user(&buf, argp, sizeof(struct mtd_oob_buf)))
-                        return -EFAULT;
-
-                if (buf.length > 4096)
-                        return -EINVAL;
-
-                if (!mtd->write_oob)
-                        ret = -EOPNOTSUPP;
-                else
-                        ret = access_ok(VERIFY_READ, buf.ptr,
-                                        buf.length) ? 0 : EFAULT;
-
-                if (ret)
-                        return ret;
-
-                ops.ooblen = buf.length;
-                ops.ooboffs = buf.start & (mtd->oobsize - 1);
-                ops.datbuf = NULL;
-                ops.mode = MTD_OOB_PLACE;
-
-                if (ops.ooboffs && ops.ooblen > (mtd->oobsize - ops.ooboffs))
-                        return -EINVAL;
-
-                ops.oobbuf = kmalloc(buf.length, GFP_KERNEL);
-                if (!ops.oobbuf)
-                        return -ENOMEM;
-
-                if (copy_from_user(ops.oobbuf, buf.ptr, buf.length)) {
-                        kfree(ops.oobbuf);
-                        return -EFAULT;
-                }
+                struct mtd_oob_buf __user *buf_user = argp;
 
-                buf.start &= ~(mtd->oobsize - 1);
-                ret = mtd->write_oob(mtd, buf.start, &ops);
-
-                if (ops.oobretlen > 0xFFFFFFFFU)
-                        ret = -EOVERFLOW;
-                retlen = ops.oobretlen;
-                if (copy_to_user(&user_buf->length, &retlen, sizeof(buf.length)))
+                /* NOTE: writes return length to buf_user->length */
+                if (copy_from_user(&buf, argp, sizeof(buf)))
                         ret = -EFAULT;
-
-                kfree(ops.oobbuf);
+                else
+                        ret = mtd_do_writeoob(file, mtd, buf.start, buf.length,
+                                buf.ptr, &buf_user->length);
                 break;
-
         }
 
         case MEMREADOOB:
         {
                 struct mtd_oob_buf buf;
-                struct mtd_oob_ops ops;
-
-                if (copy_from_user(&buf, argp, sizeof(struct mtd_oob_buf)))
-                        return -EFAULT;
-
-                if (buf.length > 4096)
-                        return -EINVAL;
+                struct mtd_oob_buf __user *buf_user = argp;
 
-                if (!mtd->read_oob)
-                        ret = -EOPNOTSUPP;
+                /* NOTE: writes return length to buf_user->start */
+                if (copy_from_user(&buf, argp, sizeof(buf)))
+                        ret = -EFAULT;
                 else
-                        ret = access_ok(VERIFY_WRITE, buf.ptr,
-                                        buf.length) ? 0 : -EFAULT;
-                if (ret)
-                        return ret;
-
-                ops.ooblen = buf.length;
-                ops.ooboffs = buf.start & (mtd->oobsize - 1);
-                ops.datbuf = NULL;
-                ops.mode = MTD_OOB_PLACE;
+                        ret = mtd_do_readoob(mtd, buf.start, buf.length,
+                                buf.ptr, &buf_user->start);
+                break;
+        }
 
-                if (ops.ooboffs && ops.ooblen > (mtd->oobsize - ops.ooboffs))
-                        return -EINVAL;
+        case MEMWRITEOOB64:
+        {
+                struct mtd_oob_buf64 buf;
+                struct mtd_oob_buf64 __user *buf_user = argp;
 
-                ops.oobbuf = kmalloc(buf.length, GFP_KERNEL);
-                if (!ops.oobbuf)
-                        return -ENOMEM;
+                if (copy_from_user(&buf, argp, sizeof(buf)))
+                        ret = -EFAULT;
+                else
+                        ret = mtd_do_writeoob(file, mtd, buf.start, buf.length,
+                                (void __user *)(uintptr_t)buf.usr_ptr,
+                                &buf_user->length);
+                break;
+        }
 
-                buf.start &= ~(mtd->oobsize - 1);
-                ret = mtd->read_oob(mtd, buf.start, &ops);
+        case MEMREADOOB64:
+        {
+                struct mtd_oob_buf64 buf;
+                struct mtd_oob_buf64 __user *buf_user = argp;
 
-                if (put_user(ops.oobretlen, (uint32_t __user *)argp))
-                        ret = -EFAULT;
-                else if (ops.oobretlen && copy_to_user(buf.ptr, ops.oobbuf,
-                                                    ops.oobretlen))
+                if (copy_from_user(&buf, argp, sizeof(buf)))
                         ret = -EFAULT;
-
-                kfree(ops.oobbuf);
+                else
+                        ret = mtd_do_readoob(mtd, buf.start, buf.length,
+                                (void __user *)(uintptr_t)buf.usr_ptr,
+                                &buf_user->length);
                 break;
         }
 
@@ -758,6 +822,68 @@ static int mtd_ioctl(struct inode *inode, struct file *file,
         return ret;
 } /* memory_ioctl */
 
+#ifdef CONFIG_COMPAT
+
+struct mtd_oob_buf32 {
+        u_int32_t start;
+        u_int32_t length;
+        compat_caddr_t ptr;     /* unsigned char* */
+};
+
+#define MEMWRITEOOB32           _IOWR('M', 3, struct mtd_oob_buf32)
+#define MEMREADOOB32            _IOWR('M', 4, struct mtd_oob_buf32)
+
+static long mtd_compat_ioctl(struct file *file, unsigned int cmd,
+        unsigned long arg)
+{
+        struct inode *inode = file->f_path.dentry->d_inode;
+        struct mtd_file_info *mfi = file->private_data;
+        struct mtd_info *mtd = mfi->mtd;
+        void __user *argp = compat_ptr(arg);
+        int ret = 0;
+
+        lock_kernel();
+
+        switch (cmd) {
+        case MEMWRITEOOB32:
+        {
+                struct mtd_oob_buf32 buf;
+                struct mtd_oob_buf32 __user *buf_user = argp;
+
+                if (copy_from_user(&buf, argp, sizeof(buf)))
+                        ret = -EFAULT;
+                else
+                        ret = mtd_do_writeoob(file, mtd, buf.start,
+                                buf.length, compat_ptr(buf.ptr),
+                                &buf_user->length);
+                break;
+        }
+
+        case MEMREADOOB32:
+        {
+                struct mtd_oob_buf32 buf;
+                struct mtd_oob_buf32 __user *buf_user = argp;
+
+                /* NOTE: writes return length to buf->start */
+                if (copy_from_user(&buf, argp, sizeof(buf)))
+                        ret = -EFAULT;
+                else
+                        ret = mtd_do_readoob(mtd, buf.start,
+                                buf.length, compat_ptr(buf.ptr),
+                                &buf_user->start);
+                break;
+        }
+        default:
+                ret = mtd_ioctl(inode, file, cmd, (unsigned long)argp);
+        }
+
+        unlock_kernel();
+
+        return ret;
+}
+
+#endif /* CONFIG_COMPAT */
+
 /*
  * try to determine where a shared mapping can be made
  * - only supported for NOMMU at the moment (MMU can't doesn't copy private
@@ -817,6 +943,9 @@ static const struct file_operations mtd_fops = {
         .read           = mtd_read,
         .write          = mtd_write,
         .ioctl          = mtd_ioctl,
+#ifdef CONFIG_COMPAT
+        .compat_ioctl   = mtd_compat_ioctl,
+#endif
         .open           = mtd_open,
         .release        = mtd_close,
         .mmap           = mtd_mmap,
diff --git a/drivers/mtd/mtdcore.c b/drivers/mtd/mtdcore.c
index bccb4b1ffc46..fac54a3fa3f1 100644
--- a/drivers/mtd/mtdcore.c
+++ b/drivers/mtd/mtdcore.c
@@ -23,8 +23,15 @@
 
 #include "mtdcore.h"
 
-
-static struct class *mtd_class;
+static int mtd_cls_suspend(struct device *dev, pm_message_t state);
+static int mtd_cls_resume(struct device *dev);
+
+static struct class mtd_class = {
+        .name = "mtd",
+        .owner = THIS_MODULE,
+        .suspend = mtd_cls_suspend,
+        .resume = mtd_cls_resume,
+};
 
 /* These are exported solely for the purpose of mtd_blkdevs.c. You
    should not use them for _anything_ else */
@@ -52,7 +59,26 @@ static void mtd_release(struct device *dev)
 
         /* remove /dev/mtdXro node if needed */
         if (index)
-                device_destroy(mtd_class, index + 1);
+                device_destroy(&mtd_class, index + 1);
+}
+
+static int mtd_cls_suspend(struct device *dev, pm_message_t state)
+{
+        struct mtd_info *mtd = dev_to_mtd(dev);
+        
+        if (mtd->suspend)
+                return mtd->suspend(mtd);
+        else
+                return 0;
+}
+
+static int mtd_cls_resume(struct device *dev)
+{
+        struct mtd_info *mtd = dev_to_mtd(dev);
+        
+        if (mtd->resume)
+                mtd->resume(mtd);
+        return 0;
 }
 
 static ssize_t mtd_type_show(struct device *dev,
@@ -269,7 +295,7 @@ int add_mtd_device(struct mtd_info *mtd)
                          * physical device.
                          */
                         mtd->dev.type = &mtd_devtype;
-                        mtd->dev.class = mtd_class;
+                        mtd->dev.class = &mtd_class;
                         mtd->dev.devt = MTD_DEVT(i);
                         dev_set_name(&mtd->dev, "mtd%d", i);
                         if (device_register(&mtd->dev) != 0) {
@@ -278,7 +304,7 @@ int add_mtd_device(struct mtd_info *mtd)
                         }
 
                         if (MTD_DEVT(i))
-                                device_create(mtd_class, mtd->dev.parent,
+                                device_create(&mtd_class, mtd->dev.parent,
                                                 MTD_DEVT(i) + 1,
                                                 NULL, "mtd%dro", i);
 
@@ -604,11 +630,12 @@ done:
 
 static int __init init_mtd(void)
 {
-        mtd_class = class_create(THIS_MODULE, "mtd");
+        int ret;
+        ret = class_register(&mtd_class);
 
-        if (IS_ERR(mtd_class)) {
-                pr_err("Error creating mtd class.\n");
-                return PTR_ERR(mtd_class);
+        if (ret) {
+                pr_err("Error registering mtd class: %d\n", ret);
+                return ret;
         }
 #ifdef CONFIG_PROC_FS
         if ((proc_mtd = create_proc_entry( "mtd", 0, NULL )))
@@ -623,7 +650,7 @@ static void __exit cleanup_mtd(void)
         if (proc_mtd)
                 remove_proc_entry( "mtd", NULL);
 #endif /* CONFIG_PROC_FS */
-        class_destroy(mtd_class);
+        class_unregister(&mtd_class);
 }
 
 module_init(init_mtd);
diff --git a/drivers/mtd/mtdpart.c b/drivers/mtd/mtdpart.c
index 29675edb44b4..349fcbe5cc0f 100644
--- a/drivers/mtd/mtdpart.c
+++ b/drivers/mtd/mtdpart.c
@@ -27,9 +27,7 @@ struct mtd_part {
         struct mtd_info mtd;
         struct mtd_info *master;
         uint64_t offset;
-        int index;
         struct list_head list;
-        int registered;
 };
 
 /*
@@ -321,8 +319,7 @@ int del_mtd_partitions(struct mtd_info *master)
         list_for_each_entry_safe(slave, next, &mtd_partitions, list)
                 if (slave->master == master) {
                         list_del(&slave->list);
-                        if (slave->registered)
-                                del_mtd_device(&slave->mtd);
+                        del_mtd_device(&slave->mtd);
                         kfree(slave);
                 }
 
@@ -395,7 +392,7 @@ static struct mtd_part *add_one_partition(struct mtd_info *master,
                 slave->mtd.get_fact_prot_info = part_get_fact_prot_info;
         if (master->sync)
                 slave->mtd.sync = part_sync;
-        if (!partno && master->suspend && master->resume) {
+        if (!partno && !master->dev.class && master->suspend && master->resume) {
                         slave->mtd.suspend = part_suspend;
                         slave->mtd.resume = part_resume;
         }
@@ -412,7 +409,6 @@ static struct mtd_part *add_one_partition(struct mtd_info *master,
         slave->mtd.erase = part_erase;
         slave->master = master;
         slave->offset = part->offset;
-        slave->index = partno;
 
         if (slave->offset == MTDPART_OFS_APPEND)
                 slave->offset = cur_offset;
@@ -500,15 +496,9 @@ static struct mtd_part *add_one_partition(struct mtd_info *master,
         }
 
 out_register:
-        if (part->mtdp) {
-                /* store the object pointer (caller may or may not register it*/
-                *part->mtdp = &slave->mtd;
-                slave->registered = 0;
-        } else {
-                /* register our partition */
-                add_mtd_device(&slave->mtd);
-                slave->registered = 1;
-        }
+        /* register our partition */
+        add_mtd_device(&slave->mtd);
+
         return slave;
 }
 
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index f3276897859e..ce96c091f01b 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -74,6 +74,12 @@ config MTD_NAND_AMS_DELTA
         help
           Support for NAND flash on Amstrad E3 (Delta).
 
+config MTD_NAND_OMAP2
+        tristate "NAND Flash device on OMAP2 and OMAP3"
+        depends on ARM && MTD_NAND && (ARCH_OMAP2 || ARCH_OMAP3)
+        help
+          Support for NAND flash on Texas Instruments OMAP2 and OMAP3 platforms.
+
 config MTD_NAND_TS7250
         tristate "NAND Flash device on TS-7250 board"
         depends on MACH_TS72XX
@@ -139,27 +145,27 @@ config MTD_NAND_PPCHAMELEONEVB
           This enables the NAND flash driver on the PPChameleon EVB Board.
 
 config MTD_NAND_S3C2410
-        tristate "NAND Flash support for S3C2410/S3C2440 SoC"
-        depends on ARCH_S3C2410
+        tristate "NAND Flash support for Samsung S3C SoCs"
+        depends on ARCH_S3C2410 || ARCH_S3C64XX
         help
-          This enables the NAND flash controller on the S3C2410 and S3C2440
+          This enables the NAND flash controller on the S3C24xx and S3C64xx
           SoCs
 
           No board specific support is done by this driver, each board
           must advertise a platform_device for the driver to attach.
 
 config MTD_NAND_S3C2410_DEBUG
-        bool "S3C2410 NAND driver debug"
+        bool "Samsung S3C NAND driver debug"
         depends on MTD_NAND_S3C2410
         help
-          Enable debugging of the S3C2410 NAND driver
+          Enable debugging of the S3C NAND driver
 
 config MTD_NAND_S3C2410_HWECC
-        bool "S3C2410 NAND Hardware ECC"
+        bool "Samsung S3C NAND Hardware ECC"
         depends on MTD_NAND_S3C2410
         help
-          Enable the use of the S3C2410's internal ECC generator when
-          using NAND. Early versions of the chip have had problems with
+          Enable the use of the controller's internal ECC generator when
+          using NAND. Early versions of the chips have had problems with
           incorrect ECC generation, and if using these, the default of
           software ECC is preferable.
 
@@ -171,7 +177,7 @@ config MTD_NAND_NDFC
          NDFC Nand Flash Controllers are integrated in IBM/AMCC's 4xx SoCs
 
 config MTD_NAND_S3C2410_CLKSTOP
-        bool "S3C2410 NAND IDLE clock stop"
+        bool "Samsung S3C NAND IDLE clock stop"
         depends on MTD_NAND_S3C2410
         default n
         help
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index d33860ac42c3..f3a786b3cff3 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -25,6 +25,7 @@ obj-$(CONFIG_MTD_NAND_CS553X)		+= cs553x_nand.o
 obj-$(CONFIG_MTD_NAND_NDFC)             += ndfc.o
 obj-$(CONFIG_MTD_NAND_ATMEL)            += atmel_nand.o
 obj-$(CONFIG_MTD_NAND_GPIO)             += gpio.o
+obj-$(CONFIG_MTD_NAND_OMAP2)            += omap2.o
 obj-$(CONFIG_MTD_NAND_CM_X270)          += cmx270_nand.o
 obj-$(CONFIG_MTD_NAND_BASLER_EXCITE)    += excite_nandflash.o
 obj-$(CONFIG_MTD_NAND_PXA3xx)           += pxa3xx_nand.o
diff --git a/drivers/mtd/nand/atmel_nand.c b/drivers/mtd/nand/atmel_nand.c
index 47a33cec3793..2802992b39da 100644
--- a/drivers/mtd/nand/atmel_nand.c
+++ b/drivers/mtd/nand/atmel_nand.c
@@ -24,6 +24,7 @@
 
 #include <linux/slab.h>
 #include <linux/module.h>
+#include <linux/moduleparam.h>
 #include <linux/platform_device.h>
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/nand.h>
@@ -47,6 +48,9 @@
 #define no_ecc          0
 #endif
 
+static int on_flash_bbt = 0;
+module_param(on_flash_bbt, int, 0);
+
 /* Register access macros */
 #define ecc_readl(add, reg)                             \
         __raw_readl(add + ATMEL_ECC_##reg)
@@ -459,12 +463,17 @@ static int __init atmel_nand_probe(struct platform_device *pdev)
 
         if (host->board->det_pin) {
                 if (gpio_get_value(host->board->det_pin)) {
-                        printk("No SmartMedia card inserted.\n");
+                        printk(KERN_INFO "No SmartMedia card inserted.\n");
                         res = ENXIO;
                         goto err_no_card;
                 }
         }
 
+        if (on_flash_bbt) {
+                printk(KERN_INFO "atmel_nand: Use On Flash BBT\n");
+                nand_chip->options |= NAND_USE_FLASH_BBT;
+        }
+
         /* first scan to find the device and get the page size */
         if (nand_scan_ident(mtd, 1)) {
                 res = -ENXIO;
diff --git a/drivers/mtd/nand/bf5xx_nand.c b/drivers/mtd/nand/bf5xx_nand.c
index 4c2a67ca801e..8506e7e606fd 100644
--- a/drivers/mtd/nand/bf5xx_nand.c
+++ b/drivers/mtd/nand/bf5xx_nand.c
@@ -458,7 +458,7 @@ static irqreturn_t bf5xx_nand_dma_irq(int irq, void *dev_id)
         return IRQ_HANDLED;
 }
 
-static int bf5xx_nand_dma_rw(struct mtd_info *mtd,
+static void bf5xx_nand_dma_rw(struct mtd_info *mtd,
                                 uint8_t *buf, int is_read)
 {
         struct bf5xx_nand_info *info = mtd_to_nand_info(mtd);
@@ -496,11 +496,20 @@ static int bf5xx_nand_dma_rw(struct mtd_info *mtd,
         /* setup DMA register with Blackfin DMA API */
         set_dma_config(CH_NFC, 0x0);
         set_dma_start_addr(CH_NFC, (unsigned long) buf);
+
+/* The DMAs have different size on BF52x and BF54x */
+#ifdef CONFIG_BF52x
+        set_dma_x_count(CH_NFC, (page_size >> 1));
+        set_dma_x_modify(CH_NFC, 2);
+        val = DI_EN | WDSIZE_16;
+#endif
+
+#ifdef CONFIG_BF54x
         set_dma_x_count(CH_NFC, (page_size >> 2));
         set_dma_x_modify(CH_NFC, 4);
-
-        /* setup write or read operation */
         val = DI_EN | WDSIZE_32;
+#endif
+        /* setup write or read operation */
         if (is_read)
                 val |= WNR;
         set_dma_config(CH_NFC, val);
@@ -512,8 +521,6 @@ static int bf5xx_nand_dma_rw(struct mtd_info *mtd,
         else
                 bfin_write_NFC_PGCTL(0x2);
         wait_for_completion(&info->dma_completion);
-
-        return 0;
 }
 
 static void bf5xx_nand_dma_read_buf(struct mtd_info *mtd,
diff --git a/drivers/mtd/nand/davinci_nand.c b/drivers/mtd/nand/davinci_nand.c
index 02700f769b8a..0fad6487e6f4 100644
--- a/drivers/mtd/nand/davinci_nand.c
+++ b/drivers/mtd/nand/davinci_nand.c
@@ -44,7 +44,7 @@
  * and some flavors of secondary chipselect (e.g. based on A12) as used
  * with multichip packages.
  *
- * The 1-bit ECC hardware is supported, but not yet the newer 4-bit ECC
+ * The 1-bit ECC hardware is supported, as well as the newer 4-bit ECC
  * available on chips like the DM355 and OMAP-L137 and needed with the
  * more error-prone MLC NAND chips.
  *
@@ -54,11 +54,14 @@
 struct davinci_nand_info {
         struct mtd_info         mtd;
         struct nand_chip        chip;
+        struct nand_ecclayout   ecclayout;
 
         struct device           *dev;
         struct clk              *clk;
         bool                    partitioned;
 
+        bool                    is_readmode;
+
         void __iomem            *base;
         void __iomem            *vaddr;
 
@@ -73,6 +76,7 @@ struct davinci_nand_info {
 };
 
 static DEFINE_SPINLOCK(davinci_nand_lock);
+static bool ecc4_busy;
 
 #define to_davinci_nand(m) container_of(m, struct davinci_nand_info, mtd)
 
@@ -218,6 +222,192 @@ static int nand_davinci_correct_1bit(struct mtd_info *mtd, u_char *dat,
 /*----------------------------------------------------------------------*/
 
 /*
+ * 4-bit hardware ECC ... context maintained over entire AEMIF
+ *
+ * This is a syndrome engine, but we avoid NAND_ECC_HW_SYNDROME
+ * since that forces use of a problematic "infix OOB" layout.
+ * Among other things, it trashes manufacturer bad block markers.
+ * Also, and specific to this hardware, it ECC-protects the "prepad"
+ * in the OOB ... while having ECC protection for parts of OOB would
+ * seem useful, the current MTD stack sometimes wants to update the
+ * OOB without recomputing ECC.
+ */
+
+static void nand_davinci_hwctl_4bit(struct mtd_info *mtd, int mode)
+{
+        struct davinci_nand_info *info = to_davinci_nand(mtd);
+        unsigned long flags;
+        u32 val;
+
+        spin_lock_irqsave(&davinci_nand_lock, flags);
+
+        /* Start 4-bit ECC calculation for read/write */
+        val = davinci_nand_readl(info, NANDFCR_OFFSET);
+        val &= ~(0x03 << 4);
+        val |= (info->core_chipsel << 4) | BIT(12);
+        davinci_nand_writel(info, NANDFCR_OFFSET, val);
+
+        info->is_readmode = (mode == NAND_ECC_READ);
+
+        spin_unlock_irqrestore(&davinci_nand_lock, flags);
+}
+
+/* Read raw ECC code after writing to NAND. */
+static void
+nand_davinci_readecc_4bit(struct davinci_nand_info *info, u32 code[4])
+{
+        const u32 mask = 0x03ff03ff;
+
+        code[0] = davinci_nand_readl(info, NAND_4BIT_ECC1_OFFSET) & mask;
+        code[1] = davinci_nand_readl(info, NAND_4BIT_ECC2_OFFSET) & mask;
+        code[2] = davinci_nand_readl(info, NAND_4BIT_ECC3_OFFSET) & mask;
+        code[3] = davinci_nand_readl(info, NAND_4BIT_ECC4_OFFSET) & mask;
+}
+
+/* Terminate read ECC; or return ECC (as bytes) of data written to NAND. */
+static int nand_davinci_calculate_4bit(struct mtd_info *mtd,
+                const u_char *dat, u_char *ecc_code)
+{
+        struct davinci_nand_info *info = to_davinci_nand(mtd);
+        u32 raw_ecc[4], *p;
+        unsigned i;
+
+        /* After a read, terminate ECC calculation by a dummy read
+         * of some 4-bit ECC register.  ECC covers everything that
+         * was read; correct() just uses the hardware state, so
+         * ecc_code is not needed.
+         */
+        if (info->is_readmode) {
+                davinci_nand_readl(info, NAND_4BIT_ECC1_OFFSET);
+                return 0;
+        }
+
+        /* Pack eight raw 10-bit ecc values into ten bytes, making
+         * two passes which each convert four values (in upper and
+         * lower halves of two 32-bit words) into five bytes.  The
+         * ROM boot loader uses this same packing scheme.
+         */
+        nand_davinci_readecc_4bit(info, raw_ecc);
+        for (i = 0, p = raw_ecc; i < 2; i++, p += 2) {
+                *ecc_code++ =   p[0]        & 0xff;
+                *ecc_code++ = ((p[0] >>  8) & 0x03) | ((p[0] >> 14) & 0xfc);
+                *ecc_code++ = ((p[0] >> 22) & 0x0f) | ((p[1] <<  4) & 0xf0);
+                *ecc_code++ = ((p[1] >>  4) & 0x3f) | ((p[1] >> 10) & 0xc0);
+                *ecc_code++ =  (p[1] >> 18) & 0xff;
+        }
+
+        return 0;
+}
+
+/* Correct up to 4 bits in data we just read, using state left in the
+ * hardware plus the ecc_code computed when it was first written.
+ */
+static int nand_davinci_correct_4bit(struct mtd_info *mtd,
+                u_char *data, u_char *ecc_code, u_char *null)
+{
+        int i;
+        struct davinci_nand_info *info = to_davinci_nand(mtd);
+        unsigned short ecc10[8];
+        unsigned short *ecc16;
+        u32 syndrome[4];
+        unsigned num_errors, corrected;
+
+        /* All bytes 0xff?  It's an erased page; ignore its ECC. */
+        for (i = 0; i < 10; i++) {
+                if (ecc_code[i] != 0xff)
+                        goto compare;
+        }
+        return 0;
+
+compare:
+        /* Unpack ten bytes into eight 10 bit values.  We know we're
+         * little-endian, and use type punning for less shifting/masking.
+         */
+        if (WARN_ON(0x01 & (unsigned) ecc_code))
+                return -EINVAL;
+        ecc16 = (unsigned short *)ecc_code;
+
+        ecc10[0] =  (ecc16[0] >>  0) & 0x3ff;
+        ecc10[1] = ((ecc16[0] >> 10) & 0x3f) | ((ecc16[1] << 6) & 0x3c0);
+        ecc10[2] =  (ecc16[1] >>  4) & 0x3ff;
+        ecc10[3] = ((ecc16[1] >> 14) & 0x3)  | ((ecc16[2] << 2) & 0x3fc);
+        ecc10[4] =  (ecc16[2] >>  8)         | ((ecc16[3] << 8) & 0x300);
+        ecc10[5] =  (ecc16[3] >>  2) & 0x3ff;
+        ecc10[6] = ((ecc16[3] >> 12) & 0xf)  | ((ecc16[4] << 4) & 0x3f0);
+        ecc10[7] =  (ecc16[4] >>  6) & 0x3ff;
+
+        /* Tell ECC controller about the expected ECC codes. */
+        for (i = 7; i >= 0; i--)
+                davinci_nand_writel(info, NAND_4BIT_ECC_LOAD_OFFSET, ecc10[i]);
+
+        /* Allow time for syndrome calculation ... then read it.
+         * A syndrome of all zeroes 0 means no detected errors.
+         */
+        davinci_nand_readl(info, NANDFSR_OFFSET);
+        nand_davinci_readecc_4bit(info, syndrome);
+        if (!(syndrome[0] | syndrome[1] | syndrome[2] | syndrome[3]))
+                return 0;
+
+        /* Start address calculation, and wait for it to complete.
+         * We _could_ start reading more data while this is working,
+         * to speed up the overall page read.
+         */
+        davinci_nand_writel(info, NANDFCR_OFFSET,
+                        davinci_nand_readl(info, NANDFCR_OFFSET) | BIT(13));
+        for (;;) {
+                u32     fsr = davinci_nand_readl(info, NANDFSR_OFFSET);
+
+                switch ((fsr >> 8) & 0x0f) {
+                case 0:         /* no error, should not happen */
+                        return 0;
+                case 1:         /* five or more errors detected */
+                        return -EIO;
+                case 2:         /* error addresses computed */
+                case 3:
+                        num_errors = 1 + ((fsr >> 16) & 0x03);
+                        goto correct;
+                default:        /* still working on it */
+                        cpu_relax();
+                        continue;
+                }
+        }
+
+correct:
+        /* correct each error */
+        for (i = 0, corrected = 0; i < num_errors; i++) {
+                int error_address, error_value;
+
+                if (i > 1) {
+                        error_address = davinci_nand_readl(info,
+                                                NAND_ERR_ADD2_OFFSET);
+                        error_value = davinci_nand_readl(info,
+                                                NAND_ERR_ERRVAL2_OFFSET);
+                } else {
+                        error_address = davinci_nand_readl(info,
+                                                NAND_ERR_ADD1_OFFSET);
+                        error_value = davinci_nand_readl(info,
+                                                NAND_ERR_ERRVAL1_OFFSET);
+                }
+
+                if (i & 1) {
+                        error_address >>= 16;
+                        error_value >>= 16;
+                }
+                error_address &= 0x3ff;
+                error_address = (512 + 7) - error_address;
+
+                if (error_address < 512) {
+                        data[error_address] ^= error_value;
+                        corrected++;
+                }
+        }
+
+        return corrected;
+}
+
+/*----------------------------------------------------------------------*/
+
+/*
  * NOTE:  NAND boot requires ALE == EM_A[1], CLE == EM_A[2], so that's
  * how these chips are normally wired.  This translates to both 8 and 16
  * bit busses using ALE == BIT(3) in byte addresses, and CLE == BIT(4).
@@ -294,6 +484,23 @@ static void __init nand_dm6446evm_flash_init(struct davinci_nand_info *info)
 
 /*----------------------------------------------------------------------*/
 
+/* An ECC layout for using 4-bit ECC with small-page flash, storing
+ * ten ECC bytes plus the manufacturer's bad block marker byte, and
+ * and not overlapping the default BBT markers.
+ */
+static struct nand_ecclayout hwecc4_small __initconst = {
+        .eccbytes = 10,
+        .eccpos = { 0, 1, 2, 3, 4,
+                /* offset 5 holds the badblock marker */
+                6, 7,
+                13, 14, 15, },
+        .oobfree = {
+                {.offset = 8, .length = 5, },
+                {.offset = 16, },
+        },
+};
+
+
 static int __init nand_davinci_probe(struct platform_device *pdev)
 {
         struct davinci_nand_pdata       *pdata = pdev->dev.platform_data;
@@ -306,6 +513,10 @@ static int __init nand_davinci_probe(struct platform_device *pdev)
         uint32_t                        val;
         nand_ecc_modes_t                ecc_mode;
 
+        /* insist on board-specific configuration */
+        if (!pdata)
+                return -ENODEV;
+
         /* which external chipselect will we be managing? */
         if (pdev->id < 0 || pdev->id > 3)
                 return -ENODEV;
@@ -351,7 +562,7 @@ static int __init nand_davinci_probe(struct platform_device *pdev)
         info->chip.select_chip  = nand_davinci_select_chip;
 
         /* options such as NAND_USE_FLASH_BBT or 16-bit widths */
-        info->chip.options      = pdata ? pdata->options : 0;
+        info->chip.options      = pdata->options;
 
         info->ioaddr            = (uint32_t __force) vaddr;
 
@@ -360,14 +571,8 @@ static int __init nand_davinci_probe(struct platform_device *pdev)
         info->mask_chipsel      = pdata->mask_chipsel;
 
         /* use nandboot-capable ALE/CLE masks by default */
-        if (pdata && pdata->mask_ale)
-                info->mask_ale  = pdata->mask_cle;
-        else
-                info->mask_ale  = MASK_ALE;
-        if (pdata && pdata->mask_cle)
-                info->mask_cle  = pdata->mask_cle;
-        else
-                info->mask_cle  = MASK_CLE;
+        info->mask_ale          = pdata->mask_cle ? : MASK_ALE;
+        info->mask_cle          = pdata->mask_cle ? : MASK_CLE;
 
         /* Set address of hardware control function */
         info->chip.cmd_ctrl     = nand_davinci_hwcontrol;
@@ -377,30 +582,44 @@ static int __init nand_davinci_probe(struct platform_device *pdev)
         info->chip.read_buf     = nand_davinci_read_buf;
         info->chip.write_buf    = nand_davinci_write_buf;
 
-        /* use board-specific ECC config; else, the best available */
-        if (pdata)
-                ecc_mode = pdata->ecc_mode;
-        else
-                ecc_mode = NAND_ECC_HW;
+        /* Use board-specific ECC config */
+        ecc_mode                = pdata->ecc_mode;
 
+        ret = -EINVAL;
         switch (ecc_mode) {
         case NAND_ECC_NONE:
         case NAND_ECC_SOFT:
+                pdata->ecc_bits = 0;
                 break;
         case NAND_ECC_HW:
-                info->chip.ecc.calculate = nand_davinci_calculate_1bit;
-                info->chip.ecc.correct = nand_davinci_correct_1bit;
-                info->chip.ecc.hwctl = nand_davinci_hwctl_1bit;
+                if (pdata->ecc_bits == 4) {
+                        /* No sanity checks:  CPUs must support this,
+                         * and the chips may not use NAND_BUSWIDTH_16.
+                         */
+
+                        /* No sharing 4-bit hardware between chipselects yet */
+                        spin_lock_irq(&davinci_nand_lock);
+                        if (ecc4_busy)
+                                ret = -EBUSY;
+                        else
+                                ecc4_busy = true;
+                        spin_unlock_irq(&davinci_nand_lock);
+
+                        if (ret == -EBUSY)
+                                goto err_ecc;
+
+                        info->chip.ecc.calculate = nand_davinci_calculate_4bit;
+                        info->chip.ecc.correct = nand_davinci_correct_4bit;
+                        info->chip.ecc.hwctl = nand_davinci_hwctl_4bit;
+                        info->chip.ecc.bytes = 10;
+                } else {
+                        info->chip.ecc.calculate = nand_davinci_calculate_1bit;
+                        info->chip.ecc.correct = nand_davinci_correct_1bit;
+                        info->chip.ecc.hwctl = nand_davinci_hwctl_1bit;
+                        info->chip.ecc.bytes = 3;
+                }
                 info->chip.ecc.size = 512;
-                info->chip.ecc.bytes = 3;
                 break;
-        case NAND_ECC_HW_SYNDROME:
-                /* FIXME implement */
-                info->chip.ecc.size = 512;
-                info->chip.ecc.bytes = 10;
-
-                dev_warn(&pdev->dev, "4-bit ECC nyet supported\n");
-                /* FALL THROUGH */
         default:
                 ret = -EINVAL;
                 goto err_ecc;
@@ -441,12 +660,56 @@ static int __init nand_davinci_probe(struct platform_device *pdev)
         spin_unlock_irq(&davinci_nand_lock);
 
         /* Scan to find existence of the device(s) */
-        ret = nand_scan(&info->mtd, pdata->mask_chipsel ? 2 : 1);
+        ret = nand_scan_ident(&info->mtd, pdata->mask_chipsel ? 2 : 1);
         if (ret < 0) {
                 dev_dbg(&pdev->dev, "no NAND chip(s) found\n");
                 goto err_scan;
         }
 
+        /* Update ECC layout if needed ... for 1-bit HW ECC, the default
+         * is OK, but it allocates 6 bytes when only 3 are needed (for
+         * each 512 bytes).  For the 4-bit HW ECC, that default is not
+         * usable:  10 bytes are needed, not 6.
+         */
+        if (pdata->ecc_bits == 4) {
+                int     chunks = info->mtd.writesize / 512;
+
+                if (!chunks || info->mtd.oobsize < 16) {
+                        dev_dbg(&pdev->dev, "too small\n");
+                        ret = -EINVAL;
+                        goto err_scan;
+                }
+
+                /* For small page chips, preserve the manufacturer's
+                 * badblock marking data ... and make sure a flash BBT
+                 * table marker fits in the free bytes.
+                 */
+                if (chunks == 1) {
+                        info->ecclayout = hwecc4_small;
+                        info->ecclayout.oobfree[1].length =
+                                info->mtd.oobsize - 16;
+                        goto syndrome_done;
+                }
+
+                /* For large page chips we'll be wanting to use a
+                 * not-yet-implemented mode that reads OOB data
+                 * before reading the body of the page, to avoid
+                 * the "infix OOB" model of NAND_ECC_HW_SYNDROME
+                 * (and preserve manufacturer badblock markings).
+                 */
+                dev_warn(&pdev->dev, "no 4-bit ECC support yet "
+                                "for large page NAND\n");
+                ret = -EIO;
+                goto err_scan;
+
+syndrome_done:
+                info->chip.ecc.layout = &info->ecclayout;
+        }
+
+        ret = nand_scan_tail(&info->mtd);
+        if (ret < 0)
+                goto err_scan;
+
         if (mtd_has_partitions()) {
                 struct mtd_partition    *mtd_parts = NULL;
                 int                     mtd_parts_nb = 0;
@@ -455,22 +718,11 @@ static int __init nand_davinci_probe(struct platform_device *pdev)
                         static const char *probes[] __initconst =
                                 { "cmdlinepart", NULL };
 
-                        const char              *master_name;
-
-                        /* Set info->mtd.name = 0 temporarily */
-                        master_name             = info->mtd.name;
-                        info->mtd.name          = (char *)0;
-
-                        /* info->mtd.name == 0, means: don't bother checking
-                           <mtd-id> */
                         mtd_parts_nb = parse_mtd_partitions(&info->mtd, probes,
                                                             &mtd_parts, 0);
-
-                        /* Restore info->mtd.name */
-                        info->mtd.name = master_name;
                 }
 
-                if (mtd_parts_nb <= 0 && pdata) {
+                if (mtd_parts_nb <= 0) {
                         mtd_parts = pdata->parts;
                         mtd_parts_nb = pdata->nr_parts;
                 }
@@ -483,7 +735,7 @@ static int __init nand_davinci_probe(struct platform_device *pdev)
                                 info->partitioned = true;
                 }
 
-        } else if (pdata && pdata->nr_parts) {
+        } else if (pdata->nr_parts) {
                 dev_warn(&pdev->dev, "ignoring %d default partitions on %s\n",
                                 pdata->nr_parts, info->mtd.name);
         }
@@ -509,6 +761,11 @@ err_scan:
 err_clk_enable:
         clk_put(info->clk);
 
+        spin_lock_irq(&davinci_nand_lock);
+        if (ecc_mode == NAND_ECC_HW_SYNDROME)
+                ecc4_busy = false;
+        spin_unlock_irq(&davinci_nand_lock);
+
 err_ecc:
 err_clk:
 err_ioremap:
@@ -532,6 +789,11 @@ static int __exit nand_davinci_remove(struct platform_device *pdev)
         else
                 status = del_mtd_device(&info->mtd);
 
+        spin_lock_irq(&davinci_nand_lock);
+        if (info->chip.ecc.mode == NAND_ECC_HW_SYNDROME)
+                ecc4_busy = false;
+        spin_unlock_irq(&davinci_nand_lock);
+
         iounmap(info->base);
         iounmap(info->vaddr);
 
diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index 40c26080ecda..76beea40d2cf 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -138,7 +138,14 @@ static struct nand_ecclayout nand_hw_eccoob_8 = {
 static struct nand_ecclayout nand_hw_eccoob_16 = {
         .eccbytes = 5,
         .eccpos = {6, 7, 8, 9, 10},
-        .oobfree = {{0, 6}, {12, 4}, }
+        .oobfree = {{0, 5}, {11, 5}, }
+};
+
+static struct nand_ecclayout nand_hw_eccoob_64 = {
+        .eccbytes = 20,
+        .eccpos = {6, 7, 8, 9, 10, 22, 23, 24, 25, 26,
+                   38, 39, 40, 41, 42, 54, 55, 56, 57, 58},
+        .oobfree = {{2, 4}, {11, 10}, {27, 10}, {43, 10}, {59, 5}, }
 };
 
 #ifdef CONFIG_MTD_PARTITIONS
@@ -192,7 +199,7 @@ static void wait_op_done(struct mxc_nand_host *host, int max_retries,
                         }
                         udelay(1);
                 }
-                if (max_retries <= 0)
+                if (max_retries < 0)
                         DEBUG(MTD_DEBUG_LEVEL0, "%s(%d): INT not set\n",
                               __func__, param);
         }
@@ -795,9 +802,13 @@ static void mxc_nand_command(struct mtd_info *mtd, unsigned command,
                 send_addr(host, (page_addr & 0xff), false);
 
                 if (host->pagesize_2k) {
-                        send_addr(host, (page_addr >> 8) & 0xFF, false);
-                        if (mtd->size >= 0x40000000)
+                        if (mtd->size >= 0x10000000) {
+                                /* paddr_8 - paddr_15 */
+                                send_addr(host, (page_addr >> 8) & 0xff, false);
                                 send_addr(host, (page_addr >> 16) & 0xff, true);
+                        } else
+                                /* paddr_8 - paddr_15 */
+                                send_addr(host, (page_addr >> 8) & 0xff, true);
                 } else {
                         /* One more address cycle for higher density devices */
                         if (mtd->size >= 0x4000000) {
@@ -923,7 +934,6 @@ static int __init mxcnd_probe(struct platform_device *pdev)
                 this->ecc.mode = NAND_ECC_HW;
                 this->ecc.size = 512;
                 this->ecc.bytes = 3;
-                this->ecc.layout = &nand_hw_eccoob_8;
                 tmp = readw(host->regs + NFC_CONFIG1);
                 tmp |= NFC_ECC_EN;
                 writew(tmp, host->regs + NFC_CONFIG1);
@@ -957,12 +967,44 @@ static int __init mxcnd_probe(struct platform_device *pdev)
                 this->ecc.layout = &nand_hw_eccoob_16;
         }
 
-        host->pagesize_2k = 0;
+        /* first scan to find the device and get the page size */
+        if (nand_scan_ident(mtd, 1)) {
+                err = -ENXIO;
+                goto escan;
+        }
 
-        /* Scan to find existence of the device */
-        if (nand_scan(mtd, 1)) {
-                DEBUG(MTD_DEBUG_LEVEL0,
-                      "MXC_ND: Unable to find any NAND device.\n");
+        host->pagesize_2k = (mtd->writesize == 2048) ? 1 : 0;
+
+        if (this->ecc.mode == NAND_ECC_HW) {
+                switch (mtd->oobsize) {
+                case 8:
+                        this->ecc.layout = &nand_hw_eccoob_8;
+                        break;
+                case 16:
+                        this->ecc.layout = &nand_hw_eccoob_16;
+                        break;
+                case 64:
+                        this->ecc.layout = &nand_hw_eccoob_64;
+                        break;
+                default:
+                        /* page size not handled by HW ECC */
+                        /* switching back to soft ECC */
+                        this->ecc.size = 512;
+                        this->ecc.bytes = 3;
+                        this->ecc.layout = &nand_hw_eccoob_8;
+                        this->ecc.mode = NAND_ECC_SOFT;
+                        this->ecc.calculate = NULL;
+                        this->ecc.correct = NULL;
+                        this->ecc.hwctl = NULL;
+                        tmp = readw(host->regs + NFC_CONFIG1);
+                        tmp &= ~NFC_ECC_EN;
+                        writew(tmp, host->regs + NFC_CONFIG1);
+                        break;
+                }
+        }
+
+        /* second phase scan */
+        if (nand_scan_tail(mtd)) {
                 err = -ENXIO;
                 goto escan;
         }
@@ -985,7 +1027,7 @@ static int __init mxcnd_probe(struct platform_device *pdev)
         return 0;
 
 escan:
-        free_irq(host->irq, NULL);
+        free_irq(host->irq, host);
 eirq:
         iounmap(host->regs);
 eres:
@@ -1005,7 +1047,7 @@ static int __devexit mxcnd_remove(struct platform_device *pdev)
         platform_set_drvdata(pdev, NULL);
 
         nand_release(&host->mtd);
-        free_irq(host->irq, NULL);
+        free_irq(host->irq, host);
         iounmap(host->regs);
         kfree(host);
 
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index 3d7ed432fa41..8c21b89d2d0c 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -2756,7 +2756,8 @@ int nand_scan_tail(struct mtd_info *mtd)
          * the out of band area
          */
         chip->ecc.layout->oobavail = 0;
-        for (i = 0; chip->ecc.layout->oobfree[i].length; i++)
+        for (i = 0; chip->ecc.layout->oobfree[i].length
+                        && i < ARRAY_SIZE(chip->ecc.layout->oobfree); i++)
                 chip->ecc.layout->oobavail +=
                         chip->ecc.layout->oobfree[i].length;
         mtd->oobavail = chip->ecc.layout->oobavail;
diff --git a/drivers/mtd/nand/nand_ecc.c b/drivers/mtd/nand/nand_ecc.c
index 868147acce2c..c0cb87d6d16e 100644
--- a/drivers/mtd/nand/nand_ecc.c
+++ b/drivers/mtd/nand/nand_ecc.c
@@ -428,8 +428,8 @@ EXPORT_SYMBOL(nand_calculate_ecc);
 int nand_correct_data(struct mtd_info *mtd, unsigned char *buf,
                       unsigned char *read_ecc, unsigned char *calc_ecc)
 {
-        unsigned char b0, b1, b2;
-        unsigned char byte_addr, bit_addr;
+        unsigned char b0, b1, b2, bit_addr;
+        unsigned int byte_addr;
         /* 256 or 512 bytes/ecc  */
         const uint32_t eccsize_mult =
                         (((struct nand_chip *)mtd->priv)->ecc.size) >> 8;
diff --git a/drivers/mtd/nand/omap2.c b/drivers/mtd/nand/omap2.c
new file mode 100644
index 000000000000..0cd76f89f4b0
--- /dev/null
+++ b/drivers/mtd/nand/omap2.c
@@ -0,0 +1,776 @@
+/*
+ * Copyright © 2004 Texas Instruments, Jian Zhang <jzhang@ti.com>
+ * Copyright © 2004 Micron Technology Inc.
+ * Copyright © 2004 David Brownell
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/partitions.h>
+#include <linux/io.h>
+
+#include <asm/dma.h>
+
+#include <mach/gpmc.h>
+#include <mach/nand.h>
+
+#define GPMC_IRQ_STATUS         0x18
+#define GPMC_ECC_CONFIG         0x1F4
+#define GPMC_ECC_CONTROL        0x1F8
+#define GPMC_ECC_SIZE_CONFIG    0x1FC
+#define GPMC_ECC1_RESULT        0x200
+
+#define DRIVER_NAME     "omap2-nand"
+
+/* size (4 KiB) for IO mapping */
+#define NAND_IO_SIZE    SZ_4K
+
+#define NAND_WP_OFF     0
+#define NAND_WP_BIT     0x00000010
+#define WR_RD_PIN_MONITORING    0x00600000
+
+#define GPMC_BUF_FULL   0x00000001
+#define GPMC_BUF_EMPTY  0x00000000
+
+#define NAND_Ecc_P1e            (1 << 0)
+#define NAND_Ecc_P2e            (1 << 1)
+#define NAND_Ecc_P4e            (1 << 2)
+#define NAND_Ecc_P8e            (1 << 3)
+#define NAND_Ecc_P16e           (1 << 4)
+#define NAND_Ecc_P32e           (1 << 5)
+#define NAND_Ecc_P64e           (1 << 6)
+#define NAND_Ecc_P128e          (1 << 7)
+#define NAND_Ecc_P256e          (1 << 8)
+#define NAND_Ecc_P512e          (1 << 9)
+#define NAND_Ecc_P1024e         (1 << 10)
+#define NAND_Ecc_P2048e         (1 << 11)
+
+#define NAND_Ecc_P1o            (1 << 16)
+#define NAND_Ecc_P2o            (1 << 17)
+#define NAND_Ecc_P4o            (1 << 18)
+#define NAND_Ecc_P8o            (1 << 19)
+#define NAND_Ecc_P16o           (1 << 20)
+#define NAND_Ecc_P32o           (1 << 21)
+#define NAND_Ecc_P64o           (1 << 22)
+#define NAND_Ecc_P128o          (1 << 23)
+#define NAND_Ecc_P256o          (1 << 24)
+#define NAND_Ecc_P512o          (1 << 25)
+#define NAND_Ecc_P1024o         (1 << 26)
+#define NAND_Ecc_P2048o         (1 << 27)
+
+#define TF(value)       (value ? 1 : 0)
+
+#define P2048e(a)       (TF(a & NAND_Ecc_P2048e)        << 0)
+#define P2048o(a)       (TF(a & NAND_Ecc_P2048o)        << 1)
+#define P1e(a)          (TF(a & NAND_Ecc_P1e)           << 2)
+#define P1o(a)          (TF(a & NAND_Ecc_P1o)           << 3)
+#define P2e(a)          (TF(a & NAND_Ecc_P2e)           << 4)
+#define P2o(a)          (TF(a & NAND_Ecc_P2o)           << 5)
+#define P4e(a)          (TF(a & NAND_Ecc_P4e)           << 6)
+#define P4o(a)          (TF(a & NAND_Ecc_P4o)           << 7)
+
+#define P8e(a)          (TF(a & NAND_Ecc_P8e)           << 0)
+#define P8o(a)          (TF(a & NAND_Ecc_P8o)           << 1)
+#define P16e(a)         (TF(a & NAND_Ecc_P16e)          << 2)
+#define P16o(a)         (TF(a & NAND_Ecc_P16o)          << 3)
+#define P32e(a)         (TF(a & NAND_Ecc_P32e)          << 4)
+#define P32o(a)         (TF(a & NAND_Ecc_P32o)          << 5)
+#define P64e(a)         (TF(a & NAND_Ecc_P64e)          << 6)
+#define P64o(a)         (TF(a & NAND_Ecc_P64o)          << 7)
+
+#define P128e(a)        (TF(a & NAND_Ecc_P128e)         << 0)
+#define P128o(a)        (TF(a & NAND_Ecc_P128o)         << 1)
+#define P256e(a)        (TF(a & NAND_Ecc_P256e)         << 2)
+#define P256o(a)        (TF(a & NAND_Ecc_P256o)         << 3)
+#define P512e(a)        (TF(a & NAND_Ecc_P512e)         << 4)
+#define P512o(a)        (TF(a & NAND_Ecc_P512o)         << 5)
+#define P1024e(a)       (TF(a & NAND_Ecc_P1024e)        << 6)
+#define P1024o(a)       (TF(a & NAND_Ecc_P1024o)        << 7)
+
+#define P8e_s(a)        (TF(a & NAND_Ecc_P8e)           << 0)
+#define P8o_s(a)        (TF(a & NAND_Ecc_P8o)           << 1)
+#define P16e_s(a)       (TF(a & NAND_Ecc_P16e)          << 2)
+#define P16o_s(a)       (TF(a & NAND_Ecc_P16o)          << 3)
+#define P1e_s(a)        (TF(a & NAND_Ecc_P1e)           << 4)
+#define P1o_s(a)        (TF(a & NAND_Ecc_P1o)           << 5)
+#define P2e_s(a)        (TF(a & NAND_Ecc_P2e)           << 6)
+#define P2o_s(a)        (TF(a & NAND_Ecc_P2o)           << 7)
+
+#define P4e_s(a)        (TF(a & NAND_Ecc_P4e)           << 0)
+#define P4o_s(a)        (TF(a & NAND_Ecc_P4o)           << 1)
+
+#ifdef CONFIG_MTD_PARTITIONS
+static const char *part_probes[] = { "cmdlinepart", NULL };
+#endif
+
+struct omap_nand_info {
+        struct nand_hw_control          controller;
+        struct omap_nand_platform_data  *pdata;
+        struct mtd_info                 mtd;
+        struct mtd_partition            *parts;
+        struct nand_chip                nand;
+        struct platform_device          *pdev;
+
+        int                             gpmc_cs;
+        unsigned long                   phys_base;
+        void __iomem                    *gpmc_cs_baseaddr;
+        void __iomem                    *gpmc_baseaddr;
+};
+
+/**
+ * omap_nand_wp - This function enable or disable the Write Protect feature
+ * @mtd: MTD device structure
+ * @mode: WP ON/OFF
+ */
+static void omap_nand_wp(struct mtd_info *mtd, int mode)
+{
+        struct omap_nand_info *info = container_of(mtd,
+                                                struct omap_nand_info, mtd);
+
+        unsigned long config = __raw_readl(info->gpmc_baseaddr + GPMC_CONFIG);
+
+        if (mode)
+                config &= ~(NAND_WP_BIT);       /* WP is ON */
+        else
+                config |= (NAND_WP_BIT);        /* WP is OFF */
+
+        __raw_writel(config, (info->gpmc_baseaddr + GPMC_CONFIG));
+}
+
+/**
+ * omap_hwcontrol - hardware specific access to control-lines
+ * @mtd: MTD device structure
+ * @cmd: command to device
+ * @ctrl:
+ * NAND_NCE: bit 0 -> don't care
+ * NAND_CLE: bit 1 -> Command Latch
+ * NAND_ALE: bit 2 -> Address Latch
+ *
+ * NOTE: boards may use different bits for these!!
+ */
+static void omap_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
+{
+        struct omap_nand_info *info = container_of(mtd,
+                                        struct omap_nand_info, mtd);
+        switch (ctrl) {
+        case NAND_CTRL_CHANGE | NAND_CTRL_CLE:
+                info->nand.IO_ADDR_W = info->gpmc_cs_baseaddr +
+                                                GPMC_CS_NAND_COMMAND;
+                info->nand.IO_ADDR_R = info->gpmc_cs_baseaddr +
+                                                GPMC_CS_NAND_DATA;
+                break;
+
+        case NAND_CTRL_CHANGE | NAND_CTRL_ALE:
+                info->nand.IO_ADDR_W = info->gpmc_cs_baseaddr +
+                                                GPMC_CS_NAND_ADDRESS;
+                info->nand.IO_ADDR_R = info->gpmc_cs_baseaddr +
+                                                GPMC_CS_NAND_DATA;
+                break;
+
+        case NAND_CTRL_CHANGE | NAND_NCE:
+                info->nand.IO_ADDR_W = info->gpmc_cs_baseaddr +
+                                                GPMC_CS_NAND_DATA;
+                info->nand.IO_ADDR_R = info->gpmc_cs_baseaddr +
+                                                GPMC_CS_NAND_DATA;
+                break;
+        }
+
+        if (cmd != NAND_CMD_NONE)
+                __raw_writeb(cmd, info->nand.IO_ADDR_W);
+}
+
+/**
+ * omap_read_buf16 - read data from NAND controller into buffer
+ * @mtd: MTD device structure
+ * @buf: buffer to store date
+ * @len: number of bytes to read
+ */
+static void omap_read_buf16(struct mtd_info *mtd, u_char *buf, int len)
+{
+        struct nand_chip *nand = mtd->priv;
+
+        __raw_readsw(nand->IO_ADDR_R, buf, len / 2);
+}
+
+/**
+ * omap_write_buf16 - write buffer to NAND controller
+ * @mtd: MTD device structure
+ * @buf: data buffer
+ * @len: number of bytes to write
+ */
+static void omap_write_buf16(struct mtd_info *mtd, const u_char * buf, int len)
+{
+        struct omap_nand_info *info = container_of(mtd,
+                                                struct omap_nand_info, mtd);
+        u16 *p = (u16 *) buf;
+
+        /* FIXME try bursts of writesw() or DMA ... */
+        len >>= 1;
+
+        while (len--) {
+                writew(*p++, info->nand.IO_ADDR_W);
+
+                while (GPMC_BUF_EMPTY == (readl(info->gpmc_baseaddr +
+                                                GPMC_STATUS) & GPMC_BUF_FULL))
+                        ;
+        }
+}
+/**
+ * omap_verify_buf - Verify chip data against buffer
+ * @mtd: MTD device structure
+ * @buf: buffer containing the data to compare
+ * @len: number of bytes to compare
+ */
+static int omap_verify_buf(struct mtd_info *mtd, const u_char * buf, int len)
+{
+        struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
+                                                        mtd);
+        u16 *p = (u16 *) buf;
+
+        len >>= 1;
+        while (len--) {
+                if (*p++ != cpu_to_le16(readw(info->nand.IO_ADDR_R)))
+                        return -EFAULT;
+        }
+
+        return 0;
+}
+
+#ifdef CONFIG_MTD_NAND_OMAP_HWECC
+/**
+ * omap_hwecc_init - Initialize the HW ECC for NAND flash in GPMC controller
+ * @mtd: MTD device structure
+ */
+static void omap_hwecc_init(struct mtd_info *mtd)
+{
+        struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
+                                                        mtd);
+        struct nand_chip *chip = mtd->priv;
+        unsigned long val = 0x0;
+
+        /* Read from ECC Control Register */
+        val = __raw_readl(info->gpmc_baseaddr + GPMC_ECC_CONTROL);
+        /* Clear all ECC | Enable Reg1 */
+        val = ((0x00000001<<8) | 0x00000001);
+        __raw_writel(val, info->gpmc_baseaddr + GPMC_ECC_CONTROL);
+
+        /* Read from ECC Size Config Register */
+        val = __raw_readl(info->gpmc_baseaddr + GPMC_ECC_SIZE_CONFIG);
+        /* ECCSIZE1=512 | Select eccResultsize[0-3] */
+        val = ((((chip->ecc.size >> 1) - 1) << 22) | (0x0000000F));
+        __raw_writel(val, info->gpmc_baseaddr + GPMC_ECC_SIZE_CONFIG);
+}
+
+/**
+ * gen_true_ecc - This function will generate true ECC value
+ * @ecc_buf: buffer to store ecc code
+ *
+ * This generated true ECC value can be used when correcting
+ * data read from NAND flash memory core
+ */
+static void gen_true_ecc(u8 *ecc_buf)
+{
+        u32 tmp = ecc_buf[0] | (ecc_buf[1] << 16) |
+                ((ecc_buf[2] & 0xF0) << 20) | ((ecc_buf[2] & 0x0F) << 8);
+
+        ecc_buf[0] = ~(P64o(tmp) | P64e(tmp) | P32o(tmp) | P32e(tmp) |
+                        P16o(tmp) | P16e(tmp) | P8o(tmp) | P8e(tmp));
+        ecc_buf[1] = ~(P1024o(tmp) | P1024e(tmp) | P512o(tmp) | P512e(tmp) |
+                        P256o(tmp) | P256e(tmp) | P128o(tmp) | P128e(tmp));
+        ecc_buf[2] = ~(P4o(tmp) | P4e(tmp) | P2o(tmp) | P2e(tmp) | P1o(tmp) |
+                        P1e(tmp) | P2048o(tmp) | P2048e(tmp));
+}
+
+/**
+ * omap_compare_ecc - Detect (2 bits) and correct (1 bit) error in data
+ * @ecc_data1:  ecc code from nand spare area
+ * @ecc_data2:  ecc code from hardware register obtained from hardware ecc
+ * @page_data:  page data
+ *
+ * This function compares two ECC's and indicates if there is an error.
+ * If the error can be corrected it will be corrected to the buffer.
+ */
+static int omap_compare_ecc(u8 *ecc_data1,      /* read from NAND memory */
+                            u8 *ecc_data2,      /* read from register */
+                            u8 *page_data)
+{
+        uint    i;
+        u8      tmp0_bit[8], tmp1_bit[8], tmp2_bit[8];
+        u8      comp0_bit[8], comp1_bit[8], comp2_bit[8];
+        u8      ecc_bit[24];
+        u8      ecc_sum = 0;
+        u8      find_bit = 0;
+        uint    find_byte = 0;
+        int     isEccFF;
+
+        isEccFF = ((*(u32 *)ecc_data1 & 0xFFFFFF) == 0xFFFFFF);
+
+        gen_true_ecc(ecc_data1);
+        gen_true_ecc(ecc_data2);
+
+        for (i = 0; i <= 2; i++) {
+                *(ecc_data1 + i) = ~(*(ecc_data1 + i));
+                *(ecc_data2 + i) = ~(*(ecc_data2 + i));
+        }
+
+        for (i = 0; i < 8; i++) {
+                tmp0_bit[i]     = *ecc_data1 % 2;
+                *ecc_data1      = *ecc_data1 / 2;
+        }
+
+        for (i = 0; i < 8; i++) {
+                tmp1_bit[i]      = *(ecc_data1 + 1) % 2;
+                *(ecc_data1 + 1) = *(ecc_data1 + 1) / 2;
+        }
+
+        for (i = 0; i < 8; i++) {
+                tmp2_bit[i]      = *(ecc_data1 + 2) % 2;
+                *(ecc_data1 + 2) = *(ecc_data1 + 2) / 2;
+        }
+
+        for (i = 0; i < 8; i++) {
+                comp0_bit[i]     = *ecc_data2 % 2;
+                *ecc_data2       = *ecc_data2 / 2;
+        }
+
+        for (i = 0; i < 8; i++) {
+                comp1_bit[i]     = *(ecc_data2 + 1) % 2;
+                *(ecc_data2 + 1) = *(ecc_data2 + 1) / 2;
+        }
+
+        for (i = 0; i < 8; i++) {
+                comp2_bit[i]     = *(ecc_data2 + 2) % 2;
+                *(ecc_data2 + 2) = *(ecc_data2 + 2) / 2;
+        }
+
+        for (i = 0; i < 6; i++)
+                ecc_bit[i] = tmp2_bit[i + 2] ^ comp2_bit[i + 2];
+
+        for (i = 0; i < 8; i++)
+                ecc_bit[i + 6] = tmp0_bit[i] ^ comp0_bit[i];
+
+        for (i = 0; i < 8; i++)
+                ecc_bit[i + 14] = tmp1_bit[i] ^ comp1_bit[i];
+
+        ecc_bit[22] = tmp2_bit[0] ^ comp2_bit[0];
+        ecc_bit[23] = tmp2_bit[1] ^ comp2_bit[1];
+
+        for (i = 0; i < 24; i++)
+                ecc_sum += ecc_bit[i];
+
+        switch (ecc_sum) {
+        case 0:
+                /* Not reached because this function is not called if
+                 *  ECC values are equal
+                 */
+                return 0;
+
+        case 1:
+                /* Uncorrectable error */
+                DEBUG(MTD_DEBUG_LEVEL0, "ECC UNCORRECTED_ERROR 1\n");
+                return -1;
+
+        case 11:
+                /* UN-Correctable error */
+                DEBUG(MTD_DEBUG_LEVEL0, "ECC UNCORRECTED_ERROR B\n");
+                return -1;
+
+        case 12:
+                /* Correctable error */
+                find_byte = (ecc_bit[23] << 8) +
+                            (ecc_bit[21] << 7) +
+                            (ecc_bit[19] << 6) +
+                            (ecc_bit[17] << 5) +
+                            (ecc_bit[15] << 4) +
+                            (ecc_bit[13] << 3) +
+                            (ecc_bit[11] << 2) +
+                            (ecc_bit[9]  << 1) +
+                            ecc_bit[7];
+
+                find_bit = (ecc_bit[5] << 2) + (ecc_bit[3] << 1) + ecc_bit[1];
+
+                DEBUG(MTD_DEBUG_LEVEL0, "Correcting single bit ECC error at "
+                                "offset: %d, bit: %d\n", find_byte, find_bit);
+
+                page_data[find_byte] ^= (1 << find_bit);
+
+                return 0;
+        default:
+                if (isEccFF) {
+                        if (ecc_data2[0] == 0 &&
+                            ecc_data2[1] == 0 &&
+                            ecc_data2[2] == 0)
+                                return 0;
+                }
+                DEBUG(MTD_DEBUG_LEVEL0, "UNCORRECTED_ERROR default\n");
+                return -1;
+        }
+}
+
+/**
+ * omap_correct_data - Compares the ECC read with HW generated ECC
+ * @mtd: MTD device structure
+ * @dat: page data
+ * @read_ecc: ecc read from nand flash
+ * @calc_ecc: ecc read from HW ECC registers
+ *
+ * Compares the ecc read from nand spare area with ECC registers values
+ * and if ECC's mismached, it will call 'omap_compare_ecc' for error detection
+ * and correction.
+ */
+static int omap_correct_data(struct mtd_info *mtd, u_char *dat,
+                                u_char *read_ecc, u_char *calc_ecc)
+{
+        struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
+                                                        mtd);
+        int blockCnt = 0, i = 0, ret = 0;
+
+        /* Ex NAND_ECC_HW12_2048 */
+        if ((info->nand.ecc.mode == NAND_ECC_HW) &&
+                        (info->nand.ecc.size  == 2048))
+                blockCnt = 4;
+        else
+                blockCnt = 1;
+
+        for (i = 0; i < blockCnt; i++) {
+                if (memcmp(read_ecc, calc_ecc, 3) != 0) {
+                        ret = omap_compare_ecc(read_ecc, calc_ecc, dat);
+                        if (ret < 0)
+                                return ret;
+                }
+                read_ecc += 3;
+                calc_ecc += 3;
+                dat      += 512;
+        }
+        return 0;
+}
+
+/**
+ * omap_calcuate_ecc - Generate non-inverted ECC bytes.
+ * @mtd: MTD device structure
+ * @dat: The pointer to data on which ecc is computed
+ * @ecc_code: The ecc_code buffer
+ *
+ * Using noninverted ECC can be considered ugly since writing a blank
+ * page ie. padding will clear the ECC bytes. This is no problem as long
+ * nobody is trying to write data on the seemingly unused page. Reading
+ * an erased page will produce an ECC mismatch between generated and read
+ * ECC bytes that has to be dealt with separately.
+ */
+static int omap_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
+                                u_char *ecc_code)
+{
+        struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
+                                                        mtd);
+        unsigned long val = 0x0;
+        unsigned long reg;
+
+        /* Start Reading from HW ECC1_Result = 0x200 */
+        reg = (unsigned long)(info->gpmc_baseaddr + GPMC_ECC1_RESULT);
+        val = __raw_readl(reg);
+        *ecc_code++ = val;          /* P128e, ..., P1e */
+        *ecc_code++ = val >> 16;    /* P128o, ..., P1o */
+        /* P2048o, P1024o, P512o, P256o, P2048e, P1024e, P512e, P256e */
+        *ecc_code++ = ((val >> 8) & 0x0f) | ((val >> 20) & 0xf0);
+        reg += 4;
+
+        return 0;
+}
+
+/**
+ * omap_enable_hwecc - This function enables the hardware ecc functionality
+ * @mtd: MTD device structure
+ * @mode: Read/Write mode
+ */
+static void omap_enable_hwecc(struct mtd_info *mtd, int mode)
+{
+        struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
+                                                        mtd);
+        struct nand_chip *chip = mtd->priv;
+        unsigned int dev_width = (chip->options & NAND_BUSWIDTH_16) ? 1 : 0;
+        unsigned long val = __raw_readl(info->gpmc_baseaddr + GPMC_ECC_CONFIG);
+
+        switch (mode) {
+        case NAND_ECC_READ:
+                __raw_writel(0x101, info->gpmc_baseaddr + GPMC_ECC_CONTROL);
+                /* (ECC 16 or 8 bit col) | ( CS  )  | ECC Enable */
+                val = (dev_width << 7) | (info->gpmc_cs << 1) | (0x1);
+                break;
+        case NAND_ECC_READSYN:
+                 __raw_writel(0x100, info->gpmc_baseaddr + GPMC_ECC_CONTROL);
+                /* (ECC 16 or 8 bit col) | ( CS  )  | ECC Enable */
+                val = (dev_width << 7) | (info->gpmc_cs << 1) | (0x1);
+                break;
+        case NAND_ECC_WRITE:
+                __raw_writel(0x101, info->gpmc_baseaddr + GPMC_ECC_CONTROL);
+                /* (ECC 16 or 8 bit col) | ( CS  )  | ECC Enable */
+                val = (dev_width << 7) | (info->gpmc_cs << 1) | (0x1);
+                break;
+        default:
+                DEBUG(MTD_DEBUG_LEVEL0, "Error: Unrecognized Mode[%d]!\n",
+                                        mode);
+                break;
+        }
+
+        __raw_writel(val, info->gpmc_baseaddr + GPMC_ECC_CONFIG);
+}
+#endif
+
+/**
+ * omap_wait - wait until the command is done
+ * @mtd: MTD device structure
+ * @chip: NAND Chip structure
+ *
+ * Wait function is called during Program and erase operations and
+ * the way it is called from MTD layer, we should wait till the NAND
+ * chip is ready after the programming/erase operation has completed.
+ *
+ * Erase can take up to 400ms and program up to 20ms according to
+ * general NAND and SmartMedia specs
+ */
+static int omap_wait(struct mtd_info *mtd, struct nand_chip *chip)
+{
+        struct nand_chip *this = mtd->priv;
+        struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
+                                                        mtd);
+        unsigned long timeo = jiffies;
+        int status, state = this->state;
+
+        if (state == FL_ERASING)
+                timeo += (HZ * 400) / 1000;
+        else
+                timeo += (HZ * 20) / 1000;
+
+        this->IO_ADDR_W = (void *) info->gpmc_cs_baseaddr +
+                                                GPMC_CS_NAND_COMMAND;
+        this->IO_ADDR_R = (void *) info->gpmc_cs_baseaddr + GPMC_CS_NAND_DATA;
+
+        __raw_writeb(NAND_CMD_STATUS & 0xFF, this->IO_ADDR_W);
+
+        while (time_before(jiffies, timeo)) {
+                status = __raw_readb(this->IO_ADDR_R);
+                if (!(status & 0x40))
+                        break;
+        }
+        return status;
+}
+
+/**
+ * omap_dev_ready - calls the platform specific dev_ready function
+ * @mtd: MTD device structure
+ */
+static int omap_dev_ready(struct mtd_info *mtd)
+{
+        struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
+                                                        mtd);
+        unsigned int val = __raw_readl(info->gpmc_baseaddr + GPMC_IRQ_STATUS);
+
+        if ((val & 0x100) == 0x100) {
+                /* Clear IRQ Interrupt */
+                val |= 0x100;
+                val &= ~(0x0);
+                __raw_writel(val, info->gpmc_baseaddr + GPMC_IRQ_STATUS);
+        } else {
+                unsigned int cnt = 0;
+                while (cnt++ < 0x1FF) {
+                        if  ((val & 0x100) == 0x100)
+                                return 0;
+                        val = __raw_readl(info->gpmc_baseaddr +
+                                                        GPMC_IRQ_STATUS);
+                }
+        }
+
+        return 1;
+}
+
+static int __devinit omap_nand_probe(struct platform_device *pdev)
+{
+        struct omap_nand_info           *info;
+        struct omap_nand_platform_data  *pdata;
+        int                             err;
+        unsigned long                   val;
+
+
+        pdata = pdev->dev.platform_data;
+        if (pdata == NULL) {
+                dev_err(&pdev->dev, "platform data missing\n");
+                return -ENODEV;
+        }
+
+        info = kzalloc(sizeof(struct omap_nand_info), GFP_KERNEL);
+        if (!info)
+                return -ENOMEM;
+
+        platform_set_drvdata(pdev, info);
+
+        spin_lock_init(&info->controller.lock);
+        init_waitqueue_head(&info->controller.wq);
+
+        info->pdev = pdev;
+
+        info->gpmc_cs           = pdata->cs;
+        info->gpmc_baseaddr     = pdata->gpmc_baseaddr;
+        info->gpmc_cs_baseaddr  = pdata->gpmc_cs_baseaddr;
+
+        info->mtd.priv          = &info->nand;
+        info->mtd.name          = dev_name(&pdev->dev);
+        info->mtd.owner         = THIS_MODULE;
+
+        err = gpmc_cs_request(info->gpmc_cs, NAND_IO_SIZE, &info->phys_base);
+        if (err < 0) {
+                dev_err(&pdev->dev, "Cannot request GPMC CS\n");
+                goto out_free_info;
+        }
+
+        /* Enable RD PIN Monitoring Reg */
+        if (pdata->dev_ready) {
+                val  = gpmc_cs_read_reg(info->gpmc_cs, GPMC_CS_CONFIG1);
+                val |= WR_RD_PIN_MONITORING;
+                gpmc_cs_write_reg(info->gpmc_cs, GPMC_CS_CONFIG1, val);
+        }
+
+        val  = gpmc_cs_read_reg(info->gpmc_cs, GPMC_CS_CONFIG7);
+        val &= ~(0xf << 8);
+        val |=  (0xc & 0xf) << 8;
+        gpmc_cs_write_reg(info->gpmc_cs, GPMC_CS_CONFIG7, val);
+
+        /* NAND write protect off */
+        omap_nand_wp(&info->mtd, NAND_WP_OFF);
+
+        if (!request_mem_region(info->phys_base, NAND_IO_SIZE,
+                                pdev->dev.driver->name)) {
+                err = -EBUSY;
+                goto out_free_cs;
+        }
+
+        info->nand.IO_ADDR_R = ioremap(info->phys_base, NAND_IO_SIZE);
+        if (!info->nand.IO_ADDR_R) {
+                err = -ENOMEM;
+                goto out_release_mem_region;
+        }
+        info->nand.controller = &info->controller;
+
+        info->nand.IO_ADDR_W = info->nand.IO_ADDR_R;
+        info->nand.cmd_ctrl  = omap_hwcontrol;
+
+        /* REVISIT:  only supports 16-bit NAND flash */
+
+        info->nand.read_buf   = omap_read_buf16;
+        info->nand.write_buf  = omap_write_buf16;
+        info->nand.verify_buf = omap_verify_buf;
+
+        /*
+         * If RDY/BSY line is connected to OMAP then use the omap ready
+         * funcrtion and the generic nand_wait function which reads the status
+         * register after monitoring the RDY/BSY line.Otherwise use a standard
+         * chip delay which is slightly more than tR (AC Timing) of the NAND
+         * device and read status register until you get a failure or success
+         */
+        if (pdata->dev_ready) {
+                info->nand.dev_ready = omap_dev_ready;
+                info->nand.chip_delay = 0;
+        } else {
+                info->nand.waitfunc = omap_wait;
+                info->nand.chip_delay = 50;
+        }
+
+        info->nand.options  |= NAND_SKIP_BBTSCAN;
+        if ((gpmc_cs_read_reg(info->gpmc_cs, GPMC_CS_CONFIG1) & 0x3000)
+                                                                == 0x1000)
+                info->nand.options  |= NAND_BUSWIDTH_16;
+
+#ifdef CONFIG_MTD_NAND_OMAP_HWECC
+        info->nand.ecc.bytes            = 3;
+        info->nand.ecc.size             = 512;
+        info->nand.ecc.calculate        = omap_calculate_ecc;
+        info->nand.ecc.hwctl            = omap_enable_hwecc;
+        info->nand.ecc.correct          = omap_correct_data;
+        info->nand.ecc.mode             = NAND_ECC_HW;
+
+        /* init HW ECC */
+        omap_hwecc_init(&info->mtd);
+#else
+        info->nand.ecc.mode = NAND_ECC_SOFT;
+#endif
+
+        /* DIP switches on some boards change between 8 and 16 bit
+         * bus widths for flash.  Try the other width if the first try fails.
+         */
+        if (nand_scan(&info->mtd, 1)) {
+                info->nand.options ^= NAND_BUSWIDTH_16;
+                if (nand_scan(&info->mtd, 1)) {
+                        err = -ENXIO;
+                        goto out_release_mem_region;
+                }
+        }
+
+#ifdef CONFIG_MTD_PARTITIONS
+        err = parse_mtd_partitions(&info->mtd, part_probes, &info->parts, 0);
+        if (err > 0)
+                add_mtd_partitions(&info->mtd, info->parts, err);
+        else if (pdata->parts)
+                add_mtd_partitions(&info->mtd, pdata->parts, pdata->nr_parts);
+        else
+#endif
+                add_mtd_device(&info->mtd);
+
+        platform_set_drvdata(pdev, &info->mtd);
+
+        return 0;
+
+out_release_mem_region:
+        release_mem_region(info->phys_base, NAND_IO_SIZE);
+out_free_cs:
+        gpmc_cs_free(info->gpmc_cs);
+out_free_info:
+        kfree(info);
+
+        return err;
+}
+
+static int omap_nand_remove(struct platform_device *pdev)
+{
+        struct mtd_info *mtd = platform_get_drvdata(pdev);
+        struct omap_nand_info *info = mtd->priv;
+
+        platform_set_drvdata(pdev, NULL);
+        /* Release NAND device, its internal structures and partitions */
+        nand_release(&info->mtd);
+        iounmap(info->nand.IO_ADDR_R);
+        kfree(&info->mtd);
+        return 0;
+}
+
+static struct platform_driver omap_nand_driver = {
+        .probe          = omap_nand_probe,
+        .remove         = omap_nand_remove,
+        .driver         = {
+                .name   = DRIVER_NAME,
+                .owner  = THIS_MODULE,
+        },
+};
+
+static int __init omap_nand_init(void)
+{
+        printk(KERN_INFO "%s driver initializing\n", DRIVER_NAME);
+        return platform_driver_register(&omap_nand_driver);
+}
+
+static void __exit omap_nand_exit(void)
+{
+        platform_driver_unregister(&omap_nand_driver);
+}
+
+module_init(omap_nand_init);
+module_exit(omap_nand_exit);
+
+MODULE_ALIAS(DRIVER_NAME);
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Glue layer for NAND flash on TI OMAP boards");
diff --git a/drivers/mtd/nand/orion_nand.c b/drivers/mtd/nand/orion_nand.c
index c2dfd3ea353d..7ad972229db4 100644
--- a/drivers/mtd/nand/orion_nand.c
+++ b/drivers/mtd/nand/orion_nand.c
@@ -47,6 +47,28 @@ static void orion_nand_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl
         writeb(cmd, nc->IO_ADDR_W + offs);
 }
 
+static void orion_nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
+{
+        struct nand_chip *chip = mtd->priv;
+        void __iomem *io_base = chip->IO_ADDR_R;
+        uint64_t *buf64;
+        int i = 0;
+
+        while (len && (unsigned long)buf & 7) {
+                *buf++ = readb(io_base);
+                len--;
+        }
+        buf64 = (uint64_t *)buf;
+        while (i < len/8) {
+                uint64_t x;
+                asm ("ldrd\t%0, [%1]" : "=r" (x) : "r" (io_base));
+                buf64[i++] = x;
+        }
+        i *= 8;
+        while (i < len)
+                buf[i++] = readb(io_base);
+}
+
 static int __init orion_nand_probe(struct platform_device *pdev)
 {
         struct mtd_info *mtd;
@@ -83,6 +105,7 @@ static int __init orion_nand_probe(struct platform_device *pdev)
         nc->priv = board;
         nc->IO_ADDR_R = nc->IO_ADDR_W = io_base;
         nc->cmd_ctrl = orion_nand_cmd_ctrl;
+        nc->read_buf = orion_nand_read_buf;
         nc->ecc.mode = NAND_ECC_SOFT;
 
         if (board->chip_delay)
diff --git a/drivers/mtd/nand/plat_nand.c b/drivers/mtd/nand/plat_nand.c
index 86e1d08eee00..4e16c6f5bdd5 100644
--- a/drivers/mtd/nand/plat_nand.c
+++ b/drivers/mtd/nand/plat_nand.c
@@ -61,6 +61,8 @@ static int __devinit plat_nand_probe(struct platform_device *pdev)
         data->chip.cmd_ctrl = pdata->ctrl.cmd_ctrl;
         data->chip.dev_ready = pdata->ctrl.dev_ready;
         data->chip.select_chip = pdata->ctrl.select_chip;
+        data->chip.write_buf = pdata->ctrl.write_buf;
+        data->chip.read_buf = pdata->ctrl.read_buf;
         data->chip.chip_delay = pdata->chip.chip_delay;
         data->chip.options |= pdata->chip.options;
 
@@ -70,6 +72,13 @@ static int __devinit plat_nand_probe(struct platform_device *pdev)
 
         platform_set_drvdata(pdev, data);
 
+        /* Handle any platform specific setup */
+        if (pdata->ctrl.probe) {
+                res = pdata->ctrl.probe(pdev);
+                if (res)
+                        goto out;
+        }
+
         /* Scan to find existance of the device */
         if (nand_scan(&data->mtd, 1)) {
                 res = -ENXIO;
@@ -86,6 +95,8 @@ static int __devinit plat_nand_probe(struct platform_device *pdev)
                         return 0;
                 }
         }
+        if (pdata->chip.set_parts)
+                pdata->chip.set_parts(data->mtd.size, &pdata->chip);
         if (pdata->chip.partitions) {
                 data->parts = pdata->chip.partitions;
                 res = add_mtd_partitions(&data->mtd, data->parts,
@@ -99,6 +110,8 @@ static int __devinit plat_nand_probe(struct platform_device *pdev)
 
         nand_release(&data->mtd);
 out:
+        if (pdata->ctrl.remove)
+                pdata->ctrl.remove(pdev);
         platform_set_drvdata(pdev, NULL);
         iounmap(data->io_base);
         kfree(data);
@@ -111,15 +124,15 @@ out:
 static int __devexit plat_nand_remove(struct platform_device *pdev)
 {
         struct plat_nand_data *data = platform_get_drvdata(pdev);
-#ifdef CONFIG_MTD_PARTITIONS
         struct platform_nand_data *pdata = pdev->dev.platform_data;
-#endif
 
         nand_release(&data->mtd);
 #ifdef CONFIG_MTD_PARTITIONS
         if (data->parts && data->parts != pdata->chip.partitions)
                 kfree(data->parts);
 #endif
+        if (pdata->ctrl.remove)
+                pdata->ctrl.remove(pdev);
         iounmap(data->io_base);
         kfree(data);
 
@@ -128,7 +141,7 @@ static int __devexit plat_nand_remove(struct platform_device *pdev)
 
 static struct platform_driver plat_nand_driver = {
         .probe          = plat_nand_probe,
-        .remove         = plat_nand_remove,
+        .remove         = __devexit_p(plat_nand_remove),
         .driver         = {
                 .name   = "gen_nand",
                 .owner  = THIS_MODULE,
diff --git a/drivers/mtd/nand/s3c2410.c b/drivers/mtd/nand/s3c2410.c
index 8e375d5fe231..11dc7e69c4fb 100644
--- a/drivers/mtd/nand/s3c2410.c
+++ b/drivers/mtd/nand/s3c2410.c
@@ -74,6 +74,14 @@ static struct nand_ecclayout nand_hw_eccoob = {
 
 struct s3c2410_nand_info;
 
+/**
+ * struct s3c2410_nand_mtd - driver MTD structure
+ * @mtd: The MTD instance to pass to the MTD layer.
+ * @chip: The NAND chip information.
+ * @set: The platform information supplied for this set of NAND chips.
+ * @info: Link back to the hardware information.
+ * @scan_res: The result from calling nand_scan_ident().
+*/
 struct s3c2410_nand_mtd {
         struct mtd_info                 mtd;
         struct nand_chip                chip;
@@ -90,6 +98,21 @@ enum s3c_cpu_type {
 
 /* overview of the s3c2410 nand state */
 
+/**
+ * struct s3c2410_nand_info - NAND controller state.
+ * @mtds: An array of MTD instances on this controoler.
+ * @platform: The platform data for this board.
+ * @device: The platform device we bound to.
+ * @area: The IO area resource that came from request_mem_region().
+ * @clk: The clock resource for this controller.
+ * @regs: The area mapped for the hardware registers described by @area.
+ * @sel_reg: Pointer to the register controlling the NAND selection.
+ * @sel_bit: The bit in @sel_reg to select the NAND chip.
+ * @mtd_count: The number of MTDs created from this controller.
+ * @save_sel: The contents of @sel_reg to be saved over suspend.
+ * @clk_rate: The clock rate from @clk.
+ * @cpu_type: The exact type of this controller.
+ */
 struct s3c2410_nand_info {
         /* mtd info */
         struct nand_hw_control          controller;
@@ -145,12 +168,19 @@ static inline int allow_clk_stop(struct s3c2410_nand_info *info)
 
 #define NS_IN_KHZ 1000000
 
+/**
+ * s3c_nand_calc_rate - calculate timing data.
+ * @wanted: The cycle time in nanoseconds.
+ * @clk: The clock rate in kHz.
+ * @max: The maximum divider value.
+ *
+ * Calculate the timing value from the given parameters.
+ */
 static int s3c_nand_calc_rate(int wanted, unsigned long clk, int max)
 {
         int result;
 
-        result = (wanted * clk) / NS_IN_KHZ;
-        result++;
+        result = DIV_ROUND_UP((wanted * clk), NS_IN_KHZ);
 
         pr_debug("result %d from %ld, %d\n", result, clk, wanted);
 
@@ -169,13 +199,21 @@ static int s3c_nand_calc_rate(int wanted, unsigned long clk, int max)
 
 /* controller setup */
 
+/**
+ * s3c2410_nand_setrate - setup controller timing information.
+ * @info: The controller instance.
+ *
+ * Given the information supplied by the platform, calculate and set
+ * the necessary timing registers in the hardware to generate the
+ * necessary timing cycles to the hardware.
+ */
 static int s3c2410_nand_setrate(struct s3c2410_nand_info *info)
 {
         struct s3c2410_platform_nand *plat = info->platform;
         int tacls_max = (info->cpu_type == TYPE_S3C2412) ? 8 : 4;
         int tacls, twrph0, twrph1;
         unsigned long clkrate = clk_get_rate(info->clk);
-        unsigned long set, cfg, mask;
+        unsigned long uninitialized_var(set), cfg, uninitialized_var(mask);
         unsigned long flags;
 
         /* calculate the timing information for the controller */
@@ -215,9 +253,9 @@ static int s3c2410_nand_setrate(struct s3c2410_nand_info *info)
 
         case TYPE_S3C2440:
         case TYPE_S3C2412:
-                mask = (S3C2410_NFCONF_TACLS(tacls_max - 1) |
-                        S3C2410_NFCONF_TWRPH0(7) |
-                        S3C2410_NFCONF_TWRPH1(7));
+                mask = (S3C2440_NFCONF_TACLS(tacls_max - 1) |
+                        S3C2440_NFCONF_TWRPH0(7) |
+                        S3C2440_NFCONF_TWRPH1(7));
 
                 set = S3C2440_NFCONF_TACLS(tacls - 1);
                 set |= S3C2440_NFCONF_TWRPH0(twrph0 - 1);
@@ -225,14 +263,9 @@ static int s3c2410_nand_setrate(struct s3c2410_nand_info *info)
                 break;
 
         default:
-                /* keep compiler happy */
-                mask = 0;
-                set = 0;
                 BUG();
         }
 
-        dev_dbg(info->device, "NF_CONF is 0x%lx\n", cfg);
-
         local_irq_save(flags);
 
         cfg = readl(info->regs + S3C2410_NFCONF);
@@ -242,9 +275,18 @@ static int s3c2410_nand_setrate(struct s3c2410_nand_info *info)
 
         local_irq_restore(flags);
 
+        dev_dbg(info->device, "NF_CONF is 0x%lx\n", cfg);
+
         return 0;
 }
 
+/**
+ * s3c2410_nand_inithw - basic hardware initialisation
+ * @info: The hardware state.
+ *
+ * Do the basic initialisation of the hardware, using s3c2410_nand_setrate()
+ * to setup the hardware access speeds and set the controller to be enabled.
+*/
 static int s3c2410_nand_inithw(struct s3c2410_nand_info *info)
 {
         int ret;
@@ -268,8 +310,19 @@ static int s3c2410_nand_inithw(struct s3c2410_nand_info *info)
         return 0;
 }
 
-/* select chip */
-
+/**
+ * s3c2410_nand_select_chip - select the given nand chip
+ * @mtd: The MTD instance for this chip.
+ * @chip: The chip number.
+ *
+ * This is called by the MTD layer to either select a given chip for the
+ * @mtd instance, or to indicate that the access has finished and the
+ * chip can be de-selected.
+ *
+ * The routine ensures that the nFCE line is correctly setup, and any
+ * platform specific selection code is called to route nFCE to the specific
+ * chip.
+ */
 static void s3c2410_nand_select_chip(struct mtd_info *mtd, int chip)
 {
         struct s3c2410_nand_info *info;
@@ -530,7 +583,16 @@ static void s3c2410_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 static void s3c2440_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
         struct s3c2410_nand_info *info = s3c2410_nand_mtd_toinfo(mtd);
-        readsl(info->regs + S3C2440_NFDATA, buf, len / 4);
+
+        readsl(info->regs + S3C2440_NFDATA, buf, len >> 2);
+
+        /* cleanup if we've got less than a word to do */
+        if (len & 3) {
+                buf += len & ~3;
+
+                for (; len & 3; len--)
+                        *buf++ = readb(info->regs + S3C2440_NFDATA);
+        }
 }
 
 static void s3c2410_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
@@ -542,7 +604,16 @@ static void s3c2410_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int
 static void s3c2440_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 {
         struct s3c2410_nand_info *info = s3c2410_nand_mtd_toinfo(mtd);
-        writesl(info->regs + S3C2440_NFDATA, buf, len / 4);
+
+        writesl(info->regs + S3C2440_NFDATA, buf, len >> 2);
+
+        /* cleanup any fractional write */
+        if (len & 3) {
+                buf += len & ~3;
+
+                for (; len & 3; len--, buf++)
+                        writeb(*buf, info->regs + S3C2440_NFDATA);
+        }
 }
 
 /* cpufreq driver support */
@@ -593,7 +664,7 @@ static inline void s3c2410_nand_cpufreq_deregister(struct s3c2410_nand_info *inf
 
 /* device management functions */
 
-static int s3c2410_nand_remove(struct platform_device *pdev)
+static int s3c24xx_nand_remove(struct platform_device *pdev)
 {
         struct s3c2410_nand_info *info = to_nand_info(pdev);
 
@@ -645,17 +716,31 @@ static int s3c2410_nand_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_MTD_PARTITIONS
+const char *part_probes[] = { "cmdlinepart", NULL };
 static int s3c2410_nand_add_partition(struct s3c2410_nand_info *info,
                                       struct s3c2410_nand_mtd *mtd,
                                       struct s3c2410_nand_set *set)
 {
+        struct mtd_partition *part_info;
+        int nr_part = 0;
+
         if (set == NULL)
                 return add_mtd_device(&mtd->mtd);
 
-        if (set->nr_partitions > 0 && set->partitions != NULL) {
-                return add_mtd_partitions(&mtd->mtd, set->partitions, set->nr_partitions);
+        if (set->nr_partitions == 0) {
+                mtd->mtd.name = set->name;
+                nr_part = parse_mtd_partitions(&mtd->mtd, part_probes,
+                                                &part_info, 0);
+        } else {
+                if (set->nr_partitions > 0 && set->partitions != NULL) {
+                        nr_part = set->nr_partitions;
+                        part_info = set->partitions;
+                }
         }
 
+        if (nr_part > 0 && part_info)
+                return add_mtd_partitions(&mtd->mtd, part_info, nr_part);
+
         return add_mtd_device(&mtd->mtd);
 }
 #else
@@ -667,11 +752,16 @@ static int s3c2410_nand_add_partition(struct s3c2410_nand_info *info,
 }
 #endif
 
-/* s3c2410_nand_init_chip
+/**
+ * s3c2410_nand_init_chip - initialise a single instance of an chip
+ * @info: The base NAND controller the chip is on.
+ * @nmtd: The new controller MTD instance to fill in.
+ * @set: The information passed from the board specific platform data.
  *
- * init a single instance of an chip
-*/
-
+ * Initialise the given @nmtd from the information in @info and @set. This
+ * readies the structure for use with the MTD layer functions by ensuring
+ * all pointers are setup and the necessary control routines selected.
+ */
 static void s3c2410_nand_init_chip(struct s3c2410_nand_info *info,
                                    struct s3c2410_nand_mtd *nmtd,
                                    struct s3c2410_nand_set *set)
@@ -757,14 +847,40 @@ static void s3c2410_nand_init_chip(struct s3c2410_nand_info *info,
 
         if (set->disable_ecc)
                 chip->ecc.mode  = NAND_ECC_NONE;
+
+        switch (chip->ecc.mode) {
+        case NAND_ECC_NONE:
+                dev_info(info->device, "NAND ECC disabled\n");
+                break;
+        case NAND_ECC_SOFT:
+                dev_info(info->device, "NAND soft ECC\n");
+                break;
+        case NAND_ECC_HW:
+                dev_info(info->device, "NAND hardware ECC\n");
+                break;
+        default:
+                dev_info(info->device, "NAND ECC UNKNOWN\n");
+                break;
+        }
+
+        /* If you use u-boot BBT creation code, specifying this flag will
+         * let the kernel fish out the BBT from the NAND, and also skip the
+         * full NAND scan that can take 1/2s or so. Little things... */
+        if (set->flash_bbt)
+                chip->options |= NAND_USE_FLASH_BBT | NAND_SKIP_BBTSCAN;
 }
 
-/* s3c2410_nand_update_chip
+/**
+ * s3c2410_nand_update_chip - post probe update
+ * @info: The controller instance.
+ * @nmtd: The driver version of the MTD instance.
  *
- * post-probe chip update, to change any items, such as the
- * layout for large page nand
- */
-
+ * This routine is called after the chip probe has succesfully completed
+ * and the relevant per-chip information updated. This call ensure that
+ * we update the internal state accordingly.
+ *
+ * The internal state is currently limited to the ECC state information.
+*/
 static void s3c2410_nand_update_chip(struct s3c2410_nand_info *info,
                                      struct s3c2410_nand_mtd *nmtd)
 {
@@ -773,33 +889,33 @@ static void s3c2410_nand_update_chip(struct s3c2410_nand_info *info,
         dev_dbg(info->device, "chip %p => page shift %d\n",
                 chip, chip->page_shift);
 
-        if (hardware_ecc) {
+        if (chip->ecc.mode != NAND_ECC_HW)
+                return;
+
                 /* change the behaviour depending on wether we are using
                  * the large or small page nand device */
 
-                if (chip->page_shift > 10) {
-                        chip->ecc.size      = 256;
-                        chip->ecc.bytes     = 3;
-                } else {
-                        chip->ecc.size      = 512;
-                        chip->ecc.bytes     = 3;
-                        chip->ecc.layout    = &nand_hw_eccoob;
-                }
+        if (chip->page_shift > 10) {
+                chip->ecc.size      = 256;
+                chip->ecc.bytes     = 3;
+        } else {
+                chip->ecc.size      = 512;
+                chip->ecc.bytes     = 3;
+                chip->ecc.layout    = &nand_hw_eccoob;
         }
 }
 
-/* s3c2410_nand_probe
+/* s3c24xx_nand_probe
  *
  * called by device layer when it finds a device matching
  * one our driver can handled. This code checks to see if
  * it can allocate all necessary resources then calls the
  * nand layer to look for devices
 */
-
-static int s3c24xx_nand_probe(struct platform_device *pdev,
-                              enum s3c_cpu_type cpu_type)
+static int s3c24xx_nand_probe(struct platform_device *pdev)
 {
         struct s3c2410_platform_nand *plat = to_nand_plat(pdev);
+        enum s3c_cpu_type cpu_type; 
         struct s3c2410_nand_info *info;
         struct s3c2410_nand_mtd *nmtd;
         struct s3c2410_nand_set *sets;
@@ -809,6 +925,8 @@ static int s3c24xx_nand_probe(struct platform_device *pdev,
         int nr_sets;
         int setno;
 
+        cpu_type = platform_get_device_id(pdev)->driver_data;
+
         pr_debug("s3c2410_nand_probe(%p)\n", pdev);
 
         info = kmalloc(sizeof(*info), GFP_KERNEL);
@@ -922,7 +1040,7 @@ static int s3c24xx_nand_probe(struct platform_device *pdev,
         return 0;
 
  exit_error:
-        s3c2410_nand_remove(pdev);
+        s3c24xx_nand_remove(pdev);
 
         if (err == 0)
                 err = -EINVAL;
@@ -983,50 +1101,33 @@ static int s3c24xx_nand_resume(struct platform_device *dev)
 
 /* driver device registration */
 
-static int s3c2410_nand_probe(struct platform_device *dev)
-{
-        return s3c24xx_nand_probe(dev, TYPE_S3C2410);
-}
-
-static int s3c2440_nand_probe(struct platform_device *dev)
-{
-        return s3c24xx_nand_probe(dev, TYPE_S3C2440);
-}
-
-static int s3c2412_nand_probe(struct platform_device *dev)
-{
-        return s3c24xx_nand_probe(dev, TYPE_S3C2412);
-}
-
-static struct platform_driver s3c2410_nand_driver = {
-        .probe          = s3c2410_nand_probe,
-        .remove         = s3c2410_nand_remove,
-        .suspend        = s3c24xx_nand_suspend,
-        .resume         = s3c24xx_nand_resume,
-        .driver         = {
-                .name   = "s3c2410-nand",
-                .owner  = THIS_MODULE,
+static struct platform_device_id s3c24xx_driver_ids[] = {
+        {
+                .name           = "s3c2410-nand",
+                .driver_data    = TYPE_S3C2410,
+        }, {
+                .name           = "s3c2440-nand",
+                .driver_data    = TYPE_S3C2440,
+        }, {
+                .name           = "s3c2412-nand",
+                .driver_data    = TYPE_S3C2412,
+        }, {
+                .name           = "s3c6400-nand",
+                .driver_data    = TYPE_S3C2412, /* compatible with 2412 */
         },
+        { }
 };
 
-static struct platform_driver s3c2440_nand_driver = {
-        .probe          = s3c2440_nand_probe,
-        .remove         = s3c2410_nand_remove,
-        .suspend        = s3c24xx_nand_suspend,
-        .resume         = s3c24xx_nand_resume,
-        .driver         = {
-                .name   = "s3c2440-nand",
-                .owner  = THIS_MODULE,
-        },
-};
+MODULE_DEVICE_TABLE(platform, s3c24xx_driver_ids);
 
-static struct platform_driver s3c2412_nand_driver = {
-        .probe          = s3c2412_nand_probe,
-        .remove         = s3c2410_nand_remove,
+static struct platform_driver s3c24xx_nand_driver = {
+        .probe          = s3c24xx_nand_probe,
+        .remove         = s3c24xx_nand_remove,
         .suspend        = s3c24xx_nand_suspend,
         .resume         = s3c24xx_nand_resume,
+        .id_table       = s3c24xx_driver_ids,
         .driver         = {
-                .name   = "s3c2412-nand",
+                .name   = "s3c24xx-nand",
                 .owner  = THIS_MODULE,
         },
 };
@@ -1035,16 +1136,12 @@ static int __init s3c2410_nand_init(void)
 {
         printk("S3C24XX NAND Driver, (c) 2004 Simtec Electronics\n");
 
-        platform_driver_register(&s3c2412_nand_driver);
-        platform_driver_register(&s3c2440_nand_driver);
-        return platform_driver_register(&s3c2410_nand_driver);
+        return platform_driver_register(&s3c24xx_nand_driver);
 }
 
 static void __exit s3c2410_nand_exit(void)
 {
-        platform_driver_unregister(&s3c2412_nand_driver);
-        platform_driver_unregister(&s3c2440_nand_driver);
-        platform_driver_unregister(&s3c2410_nand_driver);
+        platform_driver_unregister(&s3c24xx_nand_driver);
 }
 
 module_init(s3c2410_nand_init);
@@ -1053,6 +1150,3 @@ module_exit(s3c2410_nand_exit);
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Ben Dooks <ben@simtec.co.uk>");
 MODULE_DESCRIPTION("S3C24XX MTD NAND driver");
-MODULE_ALIAS("platform:s3c2410-nand");
-MODULE_ALIAS("platform:s3c2412-nand");
-MODULE_ALIAS("platform:s3c2440-nand");
diff --git a/drivers/mtd/nand/txx9ndfmc.c b/drivers/mtd/nand/txx9ndfmc.c
index 812479264896..488088eff2ca 100644
--- a/drivers/mtd/nand/txx9ndfmc.c
+++ b/drivers/mtd/nand/txx9ndfmc.c
@@ -64,7 +64,7 @@ struct txx9ndfmc_priv {
         struct nand_chip chip;
         struct mtd_info mtd;
         int cs;
-        char mtdname[BUS_ID_SIZE + 2];
+        const char *mtdname;
 };
 
 #define MAX_TXX9NDFMC_DEV       4
@@ -334,16 +334,23 @@ static int __init txx9ndfmc_probe(struct platform_device *dev)
 
                 if (plat->ch_mask != 1) {
                         txx9_priv->cs = i;
-                        sprintf(txx9_priv->mtdname, "%s.%u",
-                                dev_name(&dev->dev), i);
+                        txx9_priv->mtdname = kasprintf(GFP_KERNEL, "%s.%u",
+                                                       dev_name(&dev->dev), i);
                 } else {
                         txx9_priv->cs = -1;
-                        strcpy(txx9_priv->mtdname, dev_name(&dev->dev));
+                        txx9_priv->mtdname = kstrdup(dev_name(&dev->dev),
+                                                     GFP_KERNEL);
+                }
+                if (!txx9_priv->mtdname) {
+                        kfree(txx9_priv);
+                        dev_err(&dev->dev, "Unable to allocate MTD name.\n");
+                        continue;
                 }
                 if (plat->wide_mask & (1 << i))
                         chip->options |= NAND_BUSWIDTH_16;
 
                 if (nand_scan(mtd, 1)) {
+                        kfree(txx9_priv->mtdname);
                         kfree(txx9_priv);
                         continue;
                 }
@@ -385,6 +392,7 @@ static int __exit txx9ndfmc_remove(struct platform_device *dev)
                 kfree(drvdata->parts[i]);
 #endif
                 del_mtd_device(mtd);
+                kfree(txx9_priv->mtdname);
                 kfree(txx9_priv);
         }
         return 0;
diff --git a/drivers/mtd/onenand/omap2.c b/drivers/mtd/onenand/omap2.c
index 6391e3dc8002..38d656b9b2ee 100644
--- a/drivers/mtd/onenand/omap2.c
+++ b/drivers/mtd/onenand/omap2.c
@@ -565,7 +565,7 @@ int omap2_onenand_rephase(void)
                                       NULL, __adjust_timing);
 }
 
-static void __devexit omap2_onenand_shutdown(struct platform_device *pdev)
+static void omap2_onenand_shutdown(struct platform_device *pdev)
 {
         struct omap2_onenand *c = dev_get_drvdata(&pdev->dev);
 
@@ -777,7 +777,7 @@ static int __devexit omap2_onenand_remove(struct platform_device *pdev)
 
 static struct platform_driver omap2_onenand_driver = {
         .probe          = omap2_onenand_probe,
-        .remove         = omap2_onenand_remove,
+        .remove         = __devexit_p(omap2_onenand_remove),
         .shutdown       = omap2_onenand_shutdown,
         .driver         = {
                 .name   = DRIVER_NAME,
diff --git a/drivers/mtd/onenand/onenand_base.c b/drivers/mtd/onenand/onenand_base.c
index 30d6999e5f9f..6e829095ea9d 100644
--- a/drivers/mtd/onenand/onenand_base.c
+++ b/drivers/mtd/onenand/onenand_base.c
@@ -9,6 +9,10 @@
  *      auto-placement support, read-while load support, various fixes
  *      Copyright (C) Nokia Corporation, 2007
  *
+ *      Vishak G <vishak.g at samsung.com>, Rohit Hagargundgi <h.rohit at samsung.com>
+ *      Flex-OneNAND support
+ *      Copyright (C) Samsung Electronics, 2008
+ *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
@@ -16,6 +20,7 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/moduleparam.h>
 #include <linux/init.h>
 #include <linux/sched.h>
 #include <linux/delay.h>
@@ -27,6 +32,38 @@
 
 #include <asm/io.h>
 
+/* Default Flex-OneNAND boundary and lock respectively */
+static int flex_bdry[MAX_DIES * 2] = { -1, 0, -1, 0 };
+
+module_param_array(flex_bdry, int, NULL, 0400);
+MODULE_PARM_DESC(flex_bdry,     "SLC Boundary information for Flex-OneNAND"
+                                "Syntax:flex_bdry=DIE_BDRY,LOCK,..."
+                                "DIE_BDRY: SLC boundary of the die"
+                                "LOCK: Locking information for SLC boundary"
+                                "    : 0->Set boundary in unlocked status"
+                                "    : 1->Set boundary in locked status");
+
+/**
+ *  onenand_oob_128 - oob info for Flex-Onenand with 4KB page
+ *  For now, we expose only 64 out of 80 ecc bytes
+ */
+static struct nand_ecclayout onenand_oob_128 = {
+        .eccbytes       = 64,
+        .eccpos         = {
+                6, 7, 8, 9, 10, 11, 12, 13, 14, 15,
+                22, 23, 24, 25, 26, 27, 28, 29, 30, 31,
+                38, 39, 40, 41, 42, 43, 44, 45, 46, 47,
+                54, 55, 56, 57, 58, 59, 60, 61, 62, 63,
+                70, 71, 72, 73, 74, 75, 76, 77, 78, 79,
+                86, 87, 88, 89, 90, 91, 92, 93, 94, 95,
+                102, 103, 104, 105
+                },
+        .oobfree        = {
+                {2, 4}, {18, 4}, {34, 4}, {50, 4},
+                {66, 4}, {82, 4}, {98, 4}, {114, 4}
+        }
+};
+
 /**
  * onenand_oob_64 - oob info for large (2KB) page
  */
@@ -65,6 +102,14 @@ static const unsigned char ffchars[] = {
         0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* 48 */
         0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
         0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* 64 */
+        0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+        0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* 80 */
+        0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+        0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* 96 */
+        0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+        0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* 112 */
+        0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+        0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, /* 128 */
 };
 
 /**
@@ -171,6 +216,70 @@ static int onenand_buffer_address(int dataram1, int sectors, int count)
 }
 
 /**
+ * flexonenand_block- For given address return block number
+ * @param this         - OneNAND device structure
+ * @param addr          - Address for which block number is needed
+ */
+static unsigned flexonenand_block(struct onenand_chip *this, loff_t addr)
+{
+        unsigned boundary, blk, die = 0;
+
+        if (ONENAND_IS_DDP(this) && addr >= this->diesize[0]) {
+                die = 1;
+                addr -= this->diesize[0];
+        }
+
+        boundary = this->boundary[die];
+
+        blk = addr >> (this->erase_shift - 1);
+        if (blk > boundary)
+                blk = (blk + boundary + 1) >> 1;
+
+        blk += die ? this->density_mask : 0;
+        return blk;
+}
+
+inline unsigned onenand_block(struct onenand_chip *this, loff_t addr)
+{
+        if (!FLEXONENAND(this))
+                return addr >> this->erase_shift;
+        return flexonenand_block(this, addr);
+}
+
+/**
+ * flexonenand_addr - Return address of the block
+ * @this:               OneNAND device structure
+ * @block:              Block number on Flex-OneNAND
+ *
+ * Return address of the block
+ */
+static loff_t flexonenand_addr(struct onenand_chip *this, int block)
+{
+        loff_t ofs = 0;
+        int die = 0, boundary;
+
+        if (ONENAND_IS_DDP(this) && block >= this->density_mask) {
+                block -= this->density_mask;
+                die = 1;
+                ofs = this->diesize[0];
+        }
+
+        boundary = this->boundary[die];
+        ofs += (loff_t)block << (this->erase_shift - 1);
+        if (block > (boundary + 1))
+                ofs += (loff_t)(block - boundary - 1) << (this->erase_shift - 1);
+        return ofs;
+}
+
+loff_t onenand_addr(struct onenand_chip *this, int block)
+{
+        if (!FLEXONENAND(this))
+                return (loff_t)block << this->erase_shift;
+        return flexonenand_addr(this, block);
+}
+EXPORT_SYMBOL(onenand_addr);
+
+/**
  * onenand_get_density - [DEFAULT] Get OneNAND density
  * @param dev_id        OneNAND device ID
  *
@@ -183,6 +292,22 @@ static inline int onenand_get_density(int dev_id)
 }
 
 /**
+ * flexonenand_region - [Flex-OneNAND] Return erase region of addr
+ * @param mtd           MTD device structure
+ * @param addr          address whose erase region needs to be identified
+ */
+int flexonenand_region(struct mtd_info *mtd, loff_t addr)
+{
+        int i;
+
+        for (i = 0; i < mtd->numeraseregions; i++)
+                if (addr < mtd->eraseregions[i].offset)
+                        break;
+        return i - 1;
+}
+EXPORT_SYMBOL(flexonenand_region);
+
+/**
  * onenand_command - [DEFAULT] Send command to OneNAND device
  * @param mtd           MTD device structure
  * @param cmd           the command to be sent
@@ -207,16 +332,28 @@ static int onenand_command(struct mtd_info *mtd, int cmd, loff_t addr, size_t le
                 page = -1;
                 break;
 
+        case FLEXONENAND_CMD_PI_ACCESS:
+                /* addr contains die index */
+                block = addr * this->density_mask;
+                page = -1;
+                break;
+
         case ONENAND_CMD_ERASE:
         case ONENAND_CMD_BUFFERRAM:
         case ONENAND_CMD_OTP_ACCESS:
-                block = (int) (addr >> this->erase_shift);
+                block = onenand_block(this, addr);
                 page = -1;
                 break;
 
+        case FLEXONENAND_CMD_READ_PI:
+                cmd = ONENAND_CMD_READ;
+                block = addr * this->density_mask;
+                page = 0;
+                break;
+
         default:
-                block = (int) (addr >> this->erase_shift);
-                page = (int) (addr >> this->page_shift);
+                block = onenand_block(this, addr);
+                page = (int) (addr - onenand_addr(this, block)) >> this->page_shift;
 
                 if (ONENAND_IS_2PLANE(this)) {
                         /* Make the even block number */
@@ -236,7 +373,7 @@ static int onenand_command(struct mtd_info *mtd, int cmd, loff_t addr, size_t le
                 value = onenand_bufferram_address(this, block);
                 this->write_word(value, this->base + ONENAND_REG_START_ADDRESS2);
 
-                if (ONENAND_IS_2PLANE(this))
+                if (ONENAND_IS_MLC(this) || ONENAND_IS_2PLANE(this))
                         /* It is always BufferRAM0 */
                         ONENAND_SET_BUFFERRAM0(this);
                 else
@@ -258,13 +395,18 @@ static int onenand_command(struct mtd_info *mtd, int cmd, loff_t addr, size_t le
 
         if (page != -1) {
                 /* Now we use page size operation */
-                int sectors = 4, count = 4;
+                int sectors = 0, count = 0;
                 int dataram;
 
                 switch (cmd) {
+                case FLEXONENAND_CMD_RECOVER_LSB:
                 case ONENAND_CMD_READ:
                 case ONENAND_CMD_READOOB:
-                        dataram = ONENAND_SET_NEXT_BUFFERRAM(this);
+                        if (ONENAND_IS_MLC(this))
+                                /* It is always BufferRAM0 */
+                                dataram = ONENAND_SET_BUFFERRAM0(this);
+                        else
+                                dataram = ONENAND_SET_NEXT_BUFFERRAM(this);
                         break;
 
                 default:
@@ -293,6 +435,30 @@ static int onenand_command(struct mtd_info *mtd, int cmd, loff_t addr, size_t le
 }
 
 /**
+ * onenand_read_ecc - return ecc status
+ * @param this          onenand chip structure
+ */
+static inline int onenand_read_ecc(struct onenand_chip *this)
+{
+        int ecc, i, result = 0;
+
+        if (!FLEXONENAND(this))
+                return this->read_word(this->base + ONENAND_REG_ECC_STATUS);
+
+        for (i = 0; i < 4; i++) {
+                ecc = this->read_word(this->base + ONENAND_REG_ECC_STATUS + i);
+                if (likely(!ecc))
+                        continue;
+                if (ecc & FLEXONENAND_UNCORRECTABLE_ERROR)
+                        return ONENAND_ECC_2BIT_ALL;
+                else
+                        result = ONENAND_ECC_1BIT_ALL;
+        }
+
+        return result;
+}
+
+/**
  * onenand_wait - [DEFAULT] wait until the command is done
  * @param mtd           MTD device structure
  * @param state         state to select the max. timeout value
@@ -331,14 +497,14 @@ static int onenand_wait(struct mtd_info *mtd, int state)
          * power off recovery (POR) test, it should read ECC status first
          */
         if (interrupt & ONENAND_INT_READ) {
-                int ecc = this->read_word(this->base + ONENAND_REG_ECC_STATUS);
+                int ecc = onenand_read_ecc(this);
                 if (ecc) {
                         if (ecc & ONENAND_ECC_2BIT_ALL) {
                                 printk(KERN_ERR "onenand_wait: ECC error = 0x%04x\n", ecc);
                                 mtd->ecc_stats.failed++;
                                 return -EBADMSG;
                         } else if (ecc & ONENAND_ECC_1BIT_ALL) {
-                                printk(KERN_INFO "onenand_wait: correctable ECC error = 0x%04x\n", ecc);
+                                printk(KERN_DEBUG "onenand_wait: correctable ECC error = 0x%04x\n", ecc);
                                 mtd->ecc_stats.corrected++;
                         }
                 }
@@ -656,7 +822,7 @@ static int onenand_check_bufferram(struct mtd_info *mtd, loff_t addr)
 
         if (found && ONENAND_IS_DDP(this)) {
                 /* Select DataRAM for DDP */
-                int block = (int) (addr >> this->erase_shift);
+                int block = onenand_block(this, addr);
                 int value = onenand_bufferram_address(this, block);
                 this->write_word(value, this->base + ONENAND_REG_START_ADDRESS2);
         }
@@ -816,6 +982,149 @@ static int onenand_transfer_auto_oob(struct mtd_info *mtd, uint8_t *buf, int col
 }
 
 /**
+ * onenand_recover_lsb - [Flex-OneNAND] Recover LSB page data
+ * @param mtd           MTD device structure
+ * @param addr          address to recover
+ * @param status        return value from onenand_wait / onenand_bbt_wait
+ *
+ * MLC NAND Flash cell has paired pages - LSB page and MSB page. LSB page has
+ * lower page address and MSB page has higher page address in paired pages.
+ * If power off occurs during MSB page program, the paired LSB page data can
+ * become corrupt. LSB page recovery read is a way to read LSB page though page
+ * data are corrupted. When uncorrectable error occurs as a result of LSB page
+ * read after power up, issue LSB page recovery read.
+ */
+static int onenand_recover_lsb(struct mtd_info *mtd, loff_t addr, int status)
+{
+        struct onenand_chip *this = mtd->priv;
+        int i;
+
+        /* Recovery is only for Flex-OneNAND */
+        if (!FLEXONENAND(this))
+                return status;
+
+        /* check if we failed due to uncorrectable error */
+        if (status != -EBADMSG && status != ONENAND_BBT_READ_ECC_ERROR)
+                return status;
+
+        /* check if address lies in MLC region */
+        i = flexonenand_region(mtd, addr);
+        if (mtd->eraseregions[i].erasesize < (1 << this->erase_shift))
+                return status;
+
+        /* We are attempting to reread, so decrement stats.failed
+         * which was incremented by onenand_wait due to read failure
+         */
+        printk(KERN_INFO "onenand_recover_lsb: Attempting to recover from uncorrectable read\n");
+        mtd->ecc_stats.failed--;
+
+        /* Issue the LSB page recovery command */
+        this->command(mtd, FLEXONENAND_CMD_RECOVER_LSB, addr, this->writesize);
+        return this->wait(mtd, FL_READING);
+}
+
+/**
+ * onenand_mlc_read_ops_nolock - MLC OneNAND read main and/or out-of-band
+ * @param mtd           MTD device structure
+ * @param from          offset to read from
+ * @param ops:          oob operation description structure
+ *
+ * MLC OneNAND / Flex-OneNAND has 4KB page size and 4KB dataram.
+ * So, read-while-load is not present.
+ */
+static int onenand_mlc_read_ops_nolock(struct mtd_info *mtd, loff_t from,
+                                struct mtd_oob_ops *ops)
+{
+        struct onenand_chip *this = mtd->priv;
+        struct mtd_ecc_stats stats;
+        size_t len = ops->len;
+        size_t ooblen = ops->ooblen;
+        u_char *buf = ops->datbuf;
+        u_char *oobbuf = ops->oobbuf;
+        int read = 0, column, thislen;
+        int oobread = 0, oobcolumn, thisooblen, oobsize;
+        int ret = 0;
+        int writesize = this->writesize;
+
+        DEBUG(MTD_DEBUG_LEVEL3, "onenand_mlc_read_ops_nolock: from = 0x%08x, len = %i\n", (unsigned int) from, (int) len);
+
+        if (ops->mode == MTD_OOB_AUTO)
+                oobsize = this->ecclayout->oobavail;
+        else
+                oobsize = mtd->oobsize;
+
+        oobcolumn = from & (mtd->oobsize - 1);
+
+        /* Do not allow reads past end of device */
+        if (from + len > mtd->size) {
+                printk(KERN_ERR "onenand_mlc_read_ops_nolock: Attempt read beyond end of device\n");
+                ops->retlen = 0;
+                ops->oobretlen = 0;
+                return -EINVAL;
+        }
+
+        stats = mtd->ecc_stats;
+
+        while (read < len) {
+                cond_resched();
+
+                thislen = min_t(int, writesize, len - read);
+
+                column = from & (writesize - 1);
+                if (column + thislen > writesize)
+                        thislen = writesize - column;
+
+                if (!onenand_check_bufferram(mtd, from)) {
+                        this->command(mtd, ONENAND_CMD_READ, from, writesize);
+
+                        ret = this->wait(mtd, FL_READING);
+                        if (unlikely(ret))
+                                ret = onenand_recover_lsb(mtd, from, ret);
+                        onenand_update_bufferram(mtd, from, !ret);
+                        if (ret == -EBADMSG)
+                                ret = 0;
+                }
+
+                this->read_bufferram(mtd, ONENAND_DATARAM, buf, column, thislen);
+                if (oobbuf) {
+                        thisooblen = oobsize - oobcolumn;
+                        thisooblen = min_t(int, thisooblen, ooblen - oobread);
+
+                        if (ops->mode == MTD_OOB_AUTO)
+                                onenand_transfer_auto_oob(mtd, oobbuf, oobcolumn, thisooblen);
+                        else
+                                this->read_bufferram(mtd, ONENAND_SPARERAM, oobbuf, oobcolumn, thisooblen);
+                        oobread += thisooblen;
+                        oobbuf += thisooblen;
+                        oobcolumn = 0;
+                }
+
+                read += thislen;
+                if (read == len)
+                        break;
+
+                from += thislen;
+                buf += thislen;
+        }
+
+        /*
+         * Return success, if no ECC failures, else -EBADMSG
+         * fs driver will take care of that, because
+         * retlen == desired len and result == -EBADMSG
+         */
+        ops->retlen = read;
+        ops->oobretlen = oobread;
+
+        if (ret)
+                return ret;
+
+        if (mtd->ecc_stats.failed - stats.failed)
+                return -EBADMSG;
+
+        return mtd->ecc_stats.corrected - stats.corrected ? -EUCLEAN : 0;
+}
+
+/**
  * onenand_read_ops_nolock - [OneNAND Interface] OneNAND read main and/or out-of-band
  * @param mtd           MTD device structure
  * @param from          offset to read from
@@ -962,7 +1271,7 @@ static int onenand_read_oob_nolock(struct mtd_info *mtd, loff_t from,
         size_t len = ops->ooblen;
         mtd_oob_mode_t mode = ops->mode;
         u_char *buf = ops->oobbuf;
-        int ret = 0;
+        int ret = 0, readcmd;
 
         from += ops->ooboffs;
 
@@ -993,17 +1302,22 @@ static int onenand_read_oob_nolock(struct mtd_info *mtd, loff_t from,
 
         stats = mtd->ecc_stats;
 
+        readcmd = ONENAND_IS_MLC(this) ? ONENAND_CMD_READ : ONENAND_CMD_READOOB;
+
         while (read < len) {
                 cond_resched();
 
                 thislen = oobsize - column;
                 thislen = min_t(int, thislen, len);
 
-                this->command(mtd, ONENAND_CMD_READOOB, from, mtd->oobsize);
+                this->command(mtd, readcmd, from, mtd->oobsize);
 
                 onenand_update_bufferram(mtd, from, 0);
 
                 ret = this->wait(mtd, FL_READING);
+                if (unlikely(ret))
+                        ret = onenand_recover_lsb(mtd, from, ret);
+
                 if (ret && ret != -EBADMSG) {
                         printk(KERN_ERR "onenand_read_oob_nolock: read failed = 0x%x\n", ret);
                         break;
@@ -1053,6 +1367,7 @@ static int onenand_read_oob_nolock(struct mtd_info *mtd, loff_t from,
 static int onenand_read(struct mtd_info *mtd, loff_t from, size_t len,
         size_t *retlen, u_char *buf)
 {
+        struct onenand_chip *this = mtd->priv;
         struct mtd_oob_ops ops = {
                 .len    = len,
                 .ooblen = 0,
@@ -1062,7 +1377,9 @@ static int onenand_read(struct mtd_info *mtd, loff_t from, size_t len,
         int ret;
 
         onenand_get_device(mtd, FL_READING);
-        ret = onenand_read_ops_nolock(mtd, from, &ops);
+        ret = ONENAND_IS_MLC(this) ?
+                onenand_mlc_read_ops_nolock(mtd, from, &ops) :
+                onenand_read_ops_nolock(mtd, from, &ops);
         onenand_release_device(mtd);
 
         *retlen = ops.retlen;
@@ -1080,6 +1397,7 @@ static int onenand_read(struct mtd_info *mtd, loff_t from, size_t len,
 static int onenand_read_oob(struct mtd_info *mtd, loff_t from,
                             struct mtd_oob_ops *ops)
 {
+        struct onenand_chip *this = mtd->priv;
         int ret;
 
         switch (ops->mode) {
@@ -1094,7 +1412,9 @@ static int onenand_read_oob(struct mtd_info *mtd, loff_t from,
 
         onenand_get_device(mtd, FL_READING);
         if (ops->datbuf)
-                ret = onenand_read_ops_nolock(mtd, from, ops);
+                ret = ONENAND_IS_MLC(this) ?
+                        onenand_mlc_read_ops_nolock(mtd, from, ops) :
+                        onenand_read_ops_nolock(mtd, from, ops);
         else
                 ret = onenand_read_oob_nolock(mtd, from, ops);
         onenand_release_device(mtd);
@@ -1128,11 +1448,11 @@ static int onenand_bbt_wait(struct mtd_info *mtd, int state)
         ctrl = this->read_word(this->base + ONENAND_REG_CTRL_STATUS);
 
         if (interrupt & ONENAND_INT_READ) {
-                int ecc = this->read_word(this->base + ONENAND_REG_ECC_STATUS);
+                int ecc = onenand_read_ecc(this);
                 if (ecc & ONENAND_ECC_2BIT_ALL) {
                         printk(KERN_INFO "onenand_bbt_wait: ecc error = 0x%04x"
                                 ", controller error 0x%04x\n", ecc, ctrl);
-                        return ONENAND_BBT_READ_ERROR;
+                        return ONENAND_BBT_READ_ECC_ERROR;
                 }
         } else {
                 printk(KERN_ERR "onenand_bbt_wait: read timeout!"
@@ -1163,7 +1483,7 @@ int onenand_bbt_read_oob(struct mtd_info *mtd, loff_t from,
 {
         struct onenand_chip *this = mtd->priv;
         int read = 0, thislen, column;
-        int ret = 0;
+        int ret = 0, readcmd;
         size_t len = ops->ooblen;
         u_char *buf = ops->oobbuf;
 
@@ -1183,17 +1503,22 @@ int onenand_bbt_read_oob(struct mtd_info *mtd, loff_t from,
 
         column = from & (mtd->oobsize - 1);
 
+        readcmd = ONENAND_IS_MLC(this) ? ONENAND_CMD_READ : ONENAND_CMD_READOOB;
+
         while (read < len) {
                 cond_resched();
 
                 thislen = mtd->oobsize - column;
                 thislen = min_t(int, thislen, len);
 
-                this->command(mtd, ONENAND_CMD_READOOB, from, mtd->oobsize);
+                this->command(mtd, readcmd, from, mtd->oobsize);
 
                 onenand_update_bufferram(mtd, from, 0);
 
-                ret = onenand_bbt_wait(mtd, FL_READING);
+                ret = this->bbt_wait(mtd, FL_READING);
+                if (unlikely(ret))
+                        ret = onenand_recover_lsb(mtd, from, ret);
+
                 if (ret)
                         break;
 
@@ -1230,9 +1555,11 @@ static int onenand_verify_oob(struct mtd_info *mtd, const u_char *buf, loff_t to
 {
         struct onenand_chip *this = mtd->priv;
         u_char *oob_buf = this->oob_buf;
-        int status, i;
+        int status, i, readcmd;
 
-        this->command(mtd, ONENAND_CMD_READOOB, to, mtd->oobsize);
+        readcmd = ONENAND_IS_MLC(this) ? ONENAND_CMD_READ : ONENAND_CMD_READOOB;
+
+        this->command(mtd, readcmd, to, mtd->oobsize);
         onenand_update_bufferram(mtd, to, 0);
         status = this->wait(mtd, FL_READING);
         if (status)
@@ -1633,7 +1960,7 @@ static int onenand_write_oob_nolock(struct mtd_info *mtd, loff_t to,
 {
         struct onenand_chip *this = mtd->priv;
         int column, ret = 0, oobsize;
-        int written = 0;
+        int written = 0, oobcmd;
         u_char *oobbuf;
         size_t len = ops->ooblen;
         const u_char *buf = ops->oobbuf;
@@ -1675,6 +2002,8 @@ static int onenand_write_oob_nolock(struct mtd_info *mtd, loff_t to,
 
         oobbuf = this->oob_buf;
 
+        oobcmd = ONENAND_IS_MLC(this) ? ONENAND_CMD_PROG : ONENAND_CMD_PROGOOB;
+
         /* Loop until all data write */
         while (written < len) {
                 int thislen = min_t(int, oobsize, len - written);
@@ -1692,7 +2021,14 @@ static int onenand_write_oob_nolock(struct mtd_info *mtd, loff_t to,
                         memcpy(oobbuf + column, buf, thislen);
                 this->write_bufferram(mtd, ONENAND_SPARERAM, oobbuf, 0, mtd->oobsize);
 
-                this->command(mtd, ONENAND_CMD_PROGOOB, to, mtd->oobsize);
+                if (ONENAND_IS_MLC(this)) {
+                        /* Set main area of DataRAM to 0xff*/
+                        memset(this->page_buf, 0xff, mtd->writesize);
+                        this->write_bufferram(mtd, ONENAND_DATARAM,
+                                         this->page_buf, 0, mtd->writesize);
+                }
+
+                this->command(mtd, oobcmd, to, mtd->oobsize);
 
                 onenand_update_bufferram(mtd, to, 0);
                 if (ONENAND_IS_2PLANE(this)) {
@@ -1815,29 +2151,48 @@ static int onenand_erase(struct mtd_info *mtd, struct erase_info *instr)
 {
         struct onenand_chip *this = mtd->priv;
         unsigned int block_size;
-        loff_t addr;
-        int len;
-        int ret = 0;
+        loff_t addr = instr->addr;
+        loff_t len = instr->len;
+        int ret = 0, i;
+        struct mtd_erase_region_info *region = NULL;
+        loff_t region_end = 0;
 
         DEBUG(MTD_DEBUG_LEVEL3, "onenand_erase: start = 0x%012llx, len = %llu\n", (unsigned long long) instr->addr, (unsigned long long) instr->len);
 
-        block_size = (1 << this->erase_shift);
-
-        /* Start address must align on block boundary */
-        if (unlikely(instr->addr & (block_size - 1))) {
-                printk(KERN_ERR "onenand_erase: Unaligned address\n");
+        /* Do not allow erase past end of device */
+        if (unlikely((len + addr) > mtd->size)) {
+                printk(KERN_ERR "onenand_erase: Erase past end of device\n");
                 return -EINVAL;
         }
 
-        /* Length must align on block boundary */
-        if (unlikely(instr->len & (block_size - 1))) {
-                printk(KERN_ERR "onenand_erase: Length not block aligned\n");
-                return -EINVAL;
+        if (FLEXONENAND(this)) {
+                /* Find the eraseregion of this address */
+                i = flexonenand_region(mtd, addr);
+                region = &mtd->eraseregions[i];
+
+                block_size = region->erasesize;
+                region_end = region->offset + region->erasesize * region->numblocks;
+
+                /* Start address within region must align on block boundary.
+                 * Erase region's start offset is always block start address.
+                 */
+                if (unlikely((addr - region->offset) & (block_size - 1))) {
+                        printk(KERN_ERR "onenand_erase: Unaligned address\n");
+                        return -EINVAL;
+                }
+        } else {
+                block_size = 1 << this->erase_shift;
+
+                /* Start address must align on block boundary */
+                if (unlikely(addr & (block_size - 1))) {
+                        printk(KERN_ERR "onenand_erase: Unaligned address\n");
+                        return -EINVAL;
+                }
         }
 
-        /* Do not allow erase past end of device */
-        if (unlikely((instr->len + instr->addr) > mtd->size)) {
-                printk(KERN_ERR "onenand_erase: Erase past end of device\n");
+        /* Length must align on block boundary */
+        if (unlikely(len & (block_size - 1))) {
+                printk(KERN_ERR "onenand_erase: Length not block aligned\n");
                 return -EINVAL;
         }
 
@@ -1847,9 +2202,6 @@ static int onenand_erase(struct mtd_info *mtd, struct erase_info *instr)
         onenand_get_device(mtd, FL_ERASING);
 
         /* Loop throught the pages */
-        len = instr->len;
-        addr = instr->addr;
-
         instr->state = MTD_ERASING;
 
         while (len) {
@@ -1869,7 +2221,8 @@ static int onenand_erase(struct mtd_info *mtd, struct erase_info *instr)
                 ret = this->wait(mtd, FL_ERASING);
                 /* Check, if it is write protected */
                 if (ret) {
-                        printk(KERN_ERR "onenand_erase: Failed erase, block %d\n", (unsigned) (addr >> this->erase_shift));
+                        printk(KERN_ERR "onenand_erase: Failed erase, block %d\n",
+                                                 onenand_block(this, addr));
                         instr->state = MTD_ERASE_FAILED;
                         instr->fail_addr = addr;
                         goto erase_exit;
@@ -1877,6 +2230,22 @@ static int onenand_erase(struct mtd_info *mtd, struct erase_info *instr)
 
                 len -= block_size;
                 addr += block_size;
+
+                if (addr == region_end) {
+                        if (!len)
+                                break;
+                        region++;
+
+                        block_size = region->erasesize;
+                        region_end = region->offset + region->erasesize * region->numblocks;
+
+                        if (len & (block_size - 1)) {
+                                /* FIXME: This should be handled at MTD partitioning level. */
+                                printk(KERN_ERR "onenand_erase: Unaligned address\n");
+                                goto erase_exit;
+                        }
+                }
+
         }
 
         instr->state = MTD_ERASE_DONE;
@@ -1955,13 +2324,17 @@ static int onenand_default_block_markbad(struct mtd_info *mtd, loff_t ofs)
         int block;
 
         /* Get block number */
-        block = ((int) ofs) >> bbm->bbt_erase_shift;
+        block = onenand_block(this, ofs);
         if (bbm->bbt)
                 bbm->bbt[block >> 2] |= 0x01 << ((block & 0x03) << 1);
 
         /* We write two bytes, so we dont have to mess with 16 bit access */
         ofs += mtd->oobsize + (bbm->badblockpos & ~0x01);
-        return onenand_write_oob_nolock(mtd, ofs, &ops);
+        /* FIXME : What to do when marking SLC block in partition
+         *         with MLC erasesize? For now, it is not advisable to
+         *         create partitions containing both SLC and MLC regions.
+         */
+        return onenand_write_oob_nolock(mtd, ofs, &ops);
 }
 
 /**
@@ -2005,8 +2378,8 @@ static int onenand_do_lock_cmd(struct mtd_info *mtd, loff_t ofs, size_t len, int
         int start, end, block, value, status;
         int wp_status_mask;
 
-        start = ofs >> this->erase_shift;
-        end = len >> this->erase_shift;
+        start = onenand_block(this, ofs);
+        end = onenand_block(this, ofs + len) - 1;
 
         if (cmd == ONENAND_CMD_LOCK)
                 wp_status_mask = ONENAND_WP_LS;
@@ -2018,7 +2391,7 @@ static int onenand_do_lock_cmd(struct mtd_info *mtd, loff_t ofs, size_t len, int
                 /* Set start block address */
                 this->write_word(start, this->base + ONENAND_REG_START_BLOCK_ADDRESS);
                 /* Set end block address */
-                this->write_word(start + end - 1, this->base + ONENAND_REG_END_BLOCK_ADDRESS);
+                this->write_word(end, this->base +  ONENAND_REG_END_BLOCK_ADDRESS);
                 /* Write lock command */
                 this->command(mtd, cmd, 0, 0);
 
@@ -2039,7 +2412,7 @@ static int onenand_do_lock_cmd(struct mtd_info *mtd, loff_t ofs, size_t len, int
         }
 
         /* Block lock scheme */
-        for (block = start; block < start + end; block++) {
+        for (block = start; block < end + 1; block++) {
                 /* Set block address */
                 value = onenand_block_address(this, block);
                 this->write_word(value, this->base + ONENAND_REG_START_ADDRESS1);
@@ -2147,7 +2520,7 @@ static void onenand_unlock_all(struct mtd_info *mtd)
 {
         struct onenand_chip *this = mtd->priv;
         loff_t ofs = 0;
-        size_t len = this->chipsize;
+        loff_t len = mtd->size;
 
         if (this->options & ONENAND_HAS_UNLOCK_ALL) {
                 /* Set start block address */
@@ -2163,12 +2536,16 @@ static void onenand_unlock_all(struct mtd_info *mtd)
                     & ONENAND_CTRL_ONGO)
                         continue;
 
+                /* Don't check lock status */
+                if (this->options & ONENAND_SKIP_UNLOCK_CHECK)
+                        return;
+
                 /* Check lock status */
                 if (onenand_check_lock_status(this))
                         return;
 
                 /* Workaround for all block unlock in DDP */
-                if (ONENAND_IS_DDP(this)) {
+                if (ONENAND_IS_DDP(this) && !FLEXONENAND(this)) {
                         /* All blocks on another chip */
                         ofs = this->chipsize >> 1;
                         len = this->chipsize >> 1;
@@ -2210,7 +2587,9 @@ static int do_otp_read(struct mtd_info *mtd, loff_t from, size_t len,
         this->command(mtd, ONENAND_CMD_OTP_ACCESS, 0, 0);
         this->wait(mtd, FL_OTPING);
 
-        ret = onenand_read_ops_nolock(mtd, from, &ops);
+        ret = ONENAND_IS_MLC(this) ?
+                onenand_mlc_read_ops_nolock(mtd, from, &ops) :
+                onenand_read_ops_nolock(mtd, from, &ops);
 
         /* Exit OTP access mode */
         this->command(mtd, ONENAND_CMD_RESET, 0, 0);
@@ -2277,21 +2656,32 @@ static int do_otp_lock(struct mtd_info *mtd, loff_t from, size_t len,
                 size_t *retlen, u_char *buf)
 {
         struct onenand_chip *this = mtd->priv;
-        struct mtd_oob_ops ops = {
-                .mode = MTD_OOB_PLACE,
-                .ooblen = len,
-                .oobbuf = buf,
-                .ooboffs = 0,
-        };
+        struct mtd_oob_ops ops;
         int ret;
 
         /* Enter OTP access mode */
         this->command(mtd, ONENAND_CMD_OTP_ACCESS, 0, 0);
         this->wait(mtd, FL_OTPING);
 
-        ret = onenand_write_oob_nolock(mtd, from, &ops);
-
-        *retlen = ops.oobretlen;
+        if (FLEXONENAND(this)) {
+                /*
+                 * For Flex-OneNAND, we write lock mark to 1st word of sector 4 of
+                 * main area of page 49.
+                 */
+                ops.len = mtd->writesize;
+                ops.ooblen = 0;
+                ops.datbuf = buf;
+                ops.oobbuf = NULL;
+                ret = onenand_write_ops_nolock(mtd, mtd->writesize * 49, &ops);
+                *retlen = ops.retlen;
+        } else {
+                ops.mode = MTD_OOB_PLACE;
+                ops.ooblen = len;
+                ops.oobbuf = buf;
+                ops.ooboffs = 0;
+                ret = onenand_write_oob_nolock(mtd, from, &ops);
+                *retlen = ops.oobretlen;
+        }
 
         /* Exit OTP access mode */
         this->command(mtd, ONENAND_CMD_RESET, 0, 0);
@@ -2475,27 +2865,34 @@ static int onenand_lock_user_prot_reg(struct mtd_info *mtd, loff_t from,
                         size_t len)
 {
         struct onenand_chip *this = mtd->priv;
-        u_char *oob_buf = this->oob_buf;
+        u_char *buf = FLEXONENAND(this) ? this->page_buf : this->oob_buf;
         size_t retlen;
         int ret;
 
-        memset(oob_buf, 0xff, mtd->oobsize);
+        memset(buf, 0xff, FLEXONENAND(this) ? this->writesize
+                                                 : mtd->oobsize);
         /*
          * Note: OTP lock operation
          *       OTP block : 0xXXFC
          *       1st block : 0xXXF3 (If chip support)
          *       Both      : 0xXXF0 (If chip support)
          */
-        oob_buf[ONENAND_OTP_LOCK_OFFSET] = 0xFC;
+        if (FLEXONENAND(this))
+                buf[FLEXONENAND_OTP_LOCK_OFFSET] = 0xFC;
+        else
+                buf[ONENAND_OTP_LOCK_OFFSET] = 0xFC;
 
         /*
          * Write lock mark to 8th word of sector0 of page0 of the spare0.
          * We write 16 bytes spare area instead of 2 bytes.
+         * For Flex-OneNAND, we write lock mark to 1st word of sector 4 of
+         * main area of page 49.
          */
+
         from = 0;
-        len = 16;
+        len = FLEXONENAND(this) ? mtd->writesize : 16;
 
-        ret = onenand_otp_walk(mtd, from, len, &retlen, oob_buf, do_otp_lock, MTD_OTP_USER);
+        ret = onenand_otp_walk(mtd, from, len, &retlen, buf, do_otp_lock, MTD_OTP_USER);
 
         return ret ? : retlen;
 }
@@ -2542,6 +2939,14 @@ static void onenand_check_features(struct mtd_info *mtd)
                 break;
         }
 
+        if (ONENAND_IS_MLC(this))
+                this->options &= ~ONENAND_HAS_2PLANE;
+
+        if (FLEXONENAND(this)) {
+                this->options &= ~ONENAND_HAS_CONT_LOCK;
+                this->options |= ONENAND_HAS_UNLOCK_ALL;
+        }
+
         if (this->options & ONENAND_HAS_CONT_LOCK)
                 printk(KERN_DEBUG "Lock scheme is Continuous Lock\n");
         if (this->options & ONENAND_HAS_UNLOCK_ALL)
@@ -2559,14 +2964,16 @@ static void onenand_check_features(struct mtd_info *mtd)
  */
 static void onenand_print_device_info(int device, int version)
 {
-        int vcc, demuxed, ddp, density;
+        int vcc, demuxed, ddp, density, flexonenand;
 
         vcc = device & ONENAND_DEVICE_VCC_MASK;
         demuxed = device & ONENAND_DEVICE_IS_DEMUX;
         ddp = device & ONENAND_DEVICE_IS_DDP;
         density = onenand_get_density(device);
-        printk(KERN_INFO "%sOneNAND%s %dMB %sV 16-bit (0x%02x)\n",
-                demuxed ? "" : "Muxed ",
+        flexonenand = device & DEVICE_IS_FLEXONENAND;
+        printk(KERN_INFO "%s%sOneNAND%s %dMB %sV 16-bit (0x%02x)\n",
+                demuxed ? "" : "Muxed ",
+                flexonenand ? "Flex-" : "",
                 ddp ? "(DDP)" : "",
                 (16 << density),
                 vcc ? "2.65/3.3" : "1.8",
@@ -2576,6 +2983,7 @@ static void onenand_print_device_info(int device, int version)
 
 static const struct onenand_manufacturers onenand_manuf_ids[] = {
         {ONENAND_MFR_SAMSUNG, "Samsung"},
+        {ONENAND_MFR_NUMONYX, "Numonyx"},
 };
 
 /**
@@ -2605,6 +3013,261 @@ static int onenand_check_maf(int manuf)
 }
 
 /**
+* flexonenand_get_boundary      - Reads the SLC boundary
+* @param onenand_info           - onenand info structure
+**/
+static int flexonenand_get_boundary(struct mtd_info *mtd)
+{
+        struct onenand_chip *this = mtd->priv;
+        unsigned die, bdry;
+        int ret, syscfg, locked;
+
+        /* Disable ECC */
+        syscfg = this->read_word(this->base + ONENAND_REG_SYS_CFG1);
+        this->write_word((syscfg | 0x0100), this->base + ONENAND_REG_SYS_CFG1);
+
+        for (die = 0; die < this->dies; die++) {
+                this->command(mtd, FLEXONENAND_CMD_PI_ACCESS, die, 0);
+                this->wait(mtd, FL_SYNCING);
+
+                this->command(mtd, FLEXONENAND_CMD_READ_PI, die, 0);
+                ret = this->wait(mtd, FL_READING);
+
+                bdry = this->read_word(this->base + ONENAND_DATARAM);
+                if ((bdry >> FLEXONENAND_PI_UNLOCK_SHIFT) == 3)
+                        locked = 0;
+                else
+                        locked = 1;
+                this->boundary[die] = bdry & FLEXONENAND_PI_MASK;
+
+                this->command(mtd, ONENAND_CMD_RESET, 0, 0);
+                ret = this->wait(mtd, FL_RESETING);
+
+                printk(KERN_INFO "Die %d boundary: %d%s\n", die,
+                       this->boundary[die], locked ? "(Locked)" : "(Unlocked)");
+        }
+
+        /* Enable ECC */
+        this->write_word(syscfg, this->base + ONENAND_REG_SYS_CFG1);
+        return 0;
+}
+
+/**
+ * flexonenand_get_size - Fill up fields in onenand_chip and mtd_info
+ *                        boundary[], diesize[], mtd->size, mtd->erasesize
+ * @param mtd           - MTD device structure
+ */
+static void flexonenand_get_size(struct mtd_info *mtd)
+{
+        struct onenand_chip *this = mtd->priv;
+        int die, i, eraseshift, density;
+        int blksperdie, maxbdry;
+        loff_t ofs;
+
+        density = onenand_get_density(this->device_id);
+        blksperdie = ((loff_t)(16 << density) << 20) >> (this->erase_shift);
+        blksperdie >>= ONENAND_IS_DDP(this) ? 1 : 0;
+        maxbdry = blksperdie - 1;
+        eraseshift = this->erase_shift - 1;
+
+        mtd->numeraseregions = this->dies << 1;
+
+        /* This fills up the device boundary */
+        flexonenand_get_boundary(mtd);
+        die = ofs = 0;
+        i = -1;
+        for (; die < this->dies; die++) {
+                if (!die || this->boundary[die-1] != maxbdry) {
+                        i++;
+                        mtd->eraseregions[i].offset = ofs;
+                        mtd->eraseregions[i].erasesize = 1 << eraseshift;
+                        mtd->eraseregions[i].numblocks =
+                                                        this->boundary[die] + 1;
+                        ofs += mtd->eraseregions[i].numblocks << eraseshift;
+                        eraseshift++;
+                } else {
+                        mtd->numeraseregions -= 1;
+                        mtd->eraseregions[i].numblocks +=
+                                                        this->boundary[die] + 1;
+                        ofs += (this->boundary[die] + 1) << (eraseshift - 1);
+                }
+                if (this->boundary[die] != maxbdry) {
+                        i++;
+                        mtd->eraseregions[i].offset = ofs;
+                        mtd->eraseregions[i].erasesize = 1 << eraseshift;
+                        mtd->eraseregions[i].numblocks = maxbdry ^
+                                                         this->boundary[die];
+                        ofs += mtd->eraseregions[i].numblocks << eraseshift;
+                        eraseshift--;
+                } else
+                        mtd->numeraseregions -= 1;
+        }
+
+        /* Expose MLC erase size except when all blocks are SLC */
+        mtd->erasesize = 1 << this->erase_shift;
+        if (mtd->numeraseregions == 1)
+                mtd->erasesize >>= 1;
+
+        printk(KERN_INFO "Device has %d eraseregions\n", mtd->numeraseregions);
+        for (i = 0; i < mtd->numeraseregions; i++)
+                printk(KERN_INFO "[offset: 0x%08x, erasesize: 0x%05x,"
+                        " numblocks: %04u]\n",
+                        (unsigned int) mtd->eraseregions[i].offset,
+                        mtd->eraseregions[i].erasesize,
+                        mtd->eraseregions[i].numblocks);
+
+        for (die = 0, mtd->size = 0; die < this->dies; die++) {
+                this->diesize[die] = (loff_t)blksperdie << this->erase_shift;
+                this->diesize[die] -= (loff_t)(this->boundary[die] + 1)
+                                                 << (this->erase_shift - 1);
+                mtd->size += this->diesize[die];
+        }
+}
+
+/**
+ * flexonenand_check_blocks_erased - Check if blocks are erased
+ * @param mtd_info      - mtd info structure
+ * @param start         - first erase block to check
+ * @param end           - last erase block to check
+ *
+ * Converting an unerased block from MLC to SLC
+ * causes byte values to change. Since both data and its ECC
+ * have changed, reads on the block give uncorrectable error.
+ * This might lead to the block being detected as bad.
+ *
+ * Avoid this by ensuring that the block to be converted is
+ * erased.
+ */
+static int flexonenand_check_blocks_erased(struct mtd_info *mtd, int start, int end)
+{
+        struct onenand_chip *this = mtd->priv;
+        int i, ret;
+        int block;
+        struct mtd_oob_ops ops = {
+                .mode = MTD_OOB_PLACE,
+                .ooboffs = 0,
+                .ooblen = mtd->oobsize,
+                .datbuf = NULL,
+                .oobbuf = this->oob_buf,
+        };
+        loff_t addr;
+
+        printk(KERN_DEBUG "Check blocks from %d to %d\n", start, end);
+
+        for (block = start; block <= end; block++) {
+                addr = flexonenand_addr(this, block);
+                if (onenand_block_isbad_nolock(mtd, addr, 0))
+                        continue;
+
+                /*
+                 * Since main area write results in ECC write to spare,
+                 * it is sufficient to check only ECC bytes for change.
+                 */
+                ret = onenand_read_oob_nolock(mtd, addr, &ops);
+                if (ret)
+                        return ret;
+
+                for (i = 0; i < mtd->oobsize; i++)
+                        if (this->oob_buf[i] != 0xff)
+                                break;
+
+                if (i != mtd->oobsize) {
+                        printk(KERN_WARNING "Block %d not erased.\n", block);
+                        return 1;
+                }
+        }
+
+        return 0;
+}
+
+/**
+ * flexonenand_set_boundary     - Writes the SLC boundary
+ * @param mtd                   - mtd info structure
+ */
+int flexonenand_set_boundary(struct mtd_info *mtd, int die,
+                                    int boundary, int lock)
+{
+        struct onenand_chip *this = mtd->priv;
+        int ret, density, blksperdie, old, new, thisboundary;
+        loff_t addr;
+
+        /* Change only once for SDP Flex-OneNAND */
+        if (die && (!ONENAND_IS_DDP(this)))
+                return 0;
+
+        /* boundary value of -1 indicates no required change */
+        if (boundary < 0 || boundary == this->boundary[die])
+                return 0;
+
+        density = onenand_get_density(this->device_id);
+        blksperdie = ((16 << density) << 20) >> this->erase_shift;
+        blksperdie >>= ONENAND_IS_DDP(this) ? 1 : 0;
+
+        if (boundary >= blksperdie) {
+                printk(KERN_ERR "flexonenand_set_boundary: Invalid boundary value. "
+                                "Boundary not changed.\n");
+                return -EINVAL;
+        }
+
+        /* Check if converting blocks are erased */
+        old = this->boundary[die] + (die * this->density_mask);
+        new = boundary + (die * this->density_mask);
+        ret = flexonenand_check_blocks_erased(mtd, min(old, new) + 1, max(old, new));
+        if (ret) {
+                printk(KERN_ERR "flexonenand_set_boundary: Please erase blocks before boundary change\n");
+                return ret;
+        }
+
+        this->command(mtd, FLEXONENAND_CMD_PI_ACCESS, die, 0);
+        this->wait(mtd, FL_SYNCING);
+
+        /* Check is boundary is locked */
+        this->command(mtd, FLEXONENAND_CMD_READ_PI, die, 0);
+        ret = this->wait(mtd, FL_READING);
+
+        thisboundary = this->read_word(this->base + ONENAND_DATARAM);
+        if ((thisboundary >> FLEXONENAND_PI_UNLOCK_SHIFT) != 3) {
+                printk(KERN_ERR "flexonenand_set_boundary: boundary locked\n");
+                ret = 1;
+                goto out;
+        }
+
+        printk(KERN_INFO "flexonenand_set_boundary: Changing die %d boundary: %d%s\n",
+                        die, boundary, lock ? "(Locked)" : "(Unlocked)");
+
+        addr = die ? this->diesize[0] : 0;
+
+        boundary &= FLEXONENAND_PI_MASK;
+        boundary |= lock ? 0 : (3 << FLEXONENAND_PI_UNLOCK_SHIFT);
+
+        this->command(mtd, ONENAND_CMD_ERASE, addr, 0);
+        ret = this->wait(mtd, FL_ERASING);
+        if (ret) {
+                printk(KERN_ERR "flexonenand_set_boundary: Failed PI erase for Die %d\n", die);
+                goto out;
+        }
+
+        this->write_word(boundary, this->base + ONENAND_DATARAM);
+        this->command(mtd, ONENAND_CMD_PROG, addr, 0);
+        ret = this->wait(mtd, FL_WRITING);
+        if (ret) {
+                printk(KERN_ERR "flexonenand_set_boundary: Failed PI write for Die %d\n", die);
+                goto out;
+        }
+
+        this->command(mtd, FLEXONENAND_CMD_PI_UPDATE, die, 0);
+        ret = this->wait(mtd, FL_WRITING);
+out:
+        this->write_word(ONENAND_CMD_RESET, this->base + ONENAND_REG_COMMAND);
+        this->wait(mtd, FL_RESETING);
+        if (!ret)
+                /* Recalculate device size on boundary change*/
+                flexonenand_get_size(mtd);
+
+        return ret;
+}
+
+/**
  * onenand_probe - [OneNAND Interface] Probe the OneNAND device
  * @param mtd           MTD device structure
  *
@@ -2621,7 +3284,7 @@ static int onenand_probe(struct mtd_info *mtd)
         /* Save system configuration 1 */
         syscfg = this->read_word(this->base + ONENAND_REG_SYS_CFG1);
         /* Clear Sync. Burst Read mode to read BootRAM */
-        this->write_word((syscfg & ~ONENAND_SYS_CFG1_SYNC_READ), this->base + ONENAND_REG_SYS_CFG1);
+        this->write_word((syscfg & ~ONENAND_SYS_CFG1_SYNC_READ & ~ONENAND_SYS_CFG1_SYNC_WRITE), this->base + ONENAND_REG_SYS_CFG1);
 
         /* Send the command for reading device ID from BootRAM */
         this->write_word(ONENAND_CMD_READID, this->base + ONENAND_BOOTRAM);
@@ -2646,6 +3309,7 @@ static int onenand_probe(struct mtd_info *mtd)
         maf_id = this->read_word(this->base + ONENAND_REG_MANUFACTURER_ID);
         dev_id = this->read_word(this->base + ONENAND_REG_DEVICE_ID);
         ver_id = this->read_word(this->base + ONENAND_REG_VERSION_ID);
+        this->technology = this->read_word(this->base + ONENAND_REG_TECHNOLOGY);
 
         /* Check OneNAND device */
         if (maf_id != bram_maf_id || dev_id != bram_dev_id)
@@ -2657,29 +3321,55 @@ static int onenand_probe(struct mtd_info *mtd)
         this->version_id = ver_id;
 
         density = onenand_get_density(dev_id);
+        if (FLEXONENAND(this)) {
+                this->dies = ONENAND_IS_DDP(this) ? 2 : 1;
+                /* Maximum possible erase regions */
+                mtd->numeraseregions = this->dies << 1;
+                mtd->eraseregions = kzalloc(sizeof(struct mtd_erase_region_info)
+                                        * (this->dies << 1), GFP_KERNEL);
+                if (!mtd->eraseregions)
+                        return -ENOMEM;
+        }
+
+        /*
+         * For Flex-OneNAND, chipsize represents maximum possible device size.
+         * mtd->size represents the actual device size.
+         */
         this->chipsize = (16 << density) << 20;
-        /* Set density mask. it is used for DDP */
-        if (ONENAND_IS_DDP(this))
-                this->density_mask = (1 << (density + 6));
-        else
-                this->density_mask = 0;
 
         /* OneNAND page size & block size */
         /* The data buffer size is equal to page size */
         mtd->writesize = this->read_word(this->base + ONENAND_REG_DATA_BUFFER_SIZE);
+        /* We use the full BufferRAM */
+        if (ONENAND_IS_MLC(this))
+                mtd->writesize <<= 1;
+
         mtd->oobsize = mtd->writesize >> 5;
         /* Pages per a block are always 64 in OneNAND */
         mtd->erasesize = mtd->writesize << 6;
+        /*
+         * Flex-OneNAND SLC area has 64 pages per block.
+         * Flex-OneNAND MLC area has 128 pages per block.
+         * Expose MLC erase size to find erase_shift and page_mask.
+         */
+        if (FLEXONENAND(this))
+                mtd->erasesize <<= 1;
 
         this->erase_shift = ffs(mtd->erasesize) - 1;
         this->page_shift = ffs(mtd->writesize) - 1;
         this->page_mask = (1 << (this->erase_shift - this->page_shift)) - 1;
+        /* Set density mask. it is used for DDP */
+        if (ONENAND_IS_DDP(this))
+                this->density_mask = this->chipsize >> (this->erase_shift + 1);
         /* It's real page size */
         this->writesize = mtd->writesize;
 
         /* REVIST: Multichip handling */
 
-        mtd->size = this->chipsize;
+        if (FLEXONENAND(this))
+                flexonenand_get_size(mtd);
+        else
+                mtd->size = this->chipsize;
 
         /* Check OneNAND features */
         onenand_check_features(mtd);
@@ -2734,7 +3424,7 @@ static void onenand_resume(struct mtd_info *mtd)
  */
 int onenand_scan(struct mtd_info *mtd, int maxchips)
 {
-        int i;
+        int i, ret;
         struct onenand_chip *this = mtd->priv;
 
         if (!this->read_word)
@@ -2746,6 +3436,10 @@ int onenand_scan(struct mtd_info *mtd, int maxchips)
                 this->command = onenand_command;
         if (!this->wait)
                 onenand_setup_wait(mtd);
+        if (!this->bbt_wait)
+                this->bbt_wait = onenand_bbt_wait;
+        if (!this->unlock_all)
+                this->unlock_all = onenand_unlock_all;
 
         if (!this->read_bufferram)
                 this->read_bufferram = onenand_read_bufferram;
@@ -2796,6 +3490,10 @@ int onenand_scan(struct mtd_info *mtd, int maxchips)
          * Allow subpage writes up to oobsize.
          */
         switch (mtd->oobsize) {
+        case 128:
+                this->ecclayout = &onenand_oob_128;
+                mtd->subpage_sft = 0;
+                break;
         case 64:
                 this->ecclayout = &onenand_oob_64;
                 mtd->subpage_sft = 2;
@@ -2859,9 +3557,18 @@ int onenand_scan(struct mtd_info *mtd, int maxchips)
         mtd->owner = THIS_MODULE;
 
         /* Unlock whole block */
-        onenand_unlock_all(mtd);
+        this->unlock_all(mtd);
+
+        ret = this->scan_bbt(mtd);
+        if ((!FLEXONENAND(this)) || ret)
+                return ret;
 
-        return this->scan_bbt(mtd);
+        /* Change Flex-OneNAND boundaries if required */
+        for (i = 0; i < MAX_DIES; i++)
+                flexonenand_set_boundary(mtd, i, flex_bdry[2 * i],
+                                                 flex_bdry[(2 * i) + 1]);
+
+        return 0;
 }
 
 /**
@@ -2890,6 +3597,7 @@ void onenand_release(struct mtd_info *mtd)
                 kfree(this->page_buf);
         if (this->options & ONENAND_OOBBUF_ALLOC)
                 kfree(this->oob_buf);
+        kfree(mtd->eraseregions);
 }
 
 EXPORT_SYMBOL_GPL(onenand_scan);
diff --git a/drivers/mtd/onenand/onenand_bbt.c b/drivers/mtd/onenand/onenand_bbt.c
index 2f53b51c6805..a91fcac1af01 100644
--- a/drivers/mtd/onenand/onenand_bbt.c
+++ b/drivers/mtd/onenand/onenand_bbt.c
@@ -63,6 +63,7 @@ static int create_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr
         loff_t from;
         size_t readlen, ooblen;
         struct mtd_oob_ops ops;
+        int rgn;
 
         printk(KERN_INFO "Scanning device for bad blocks\n");
 
@@ -76,7 +77,7 @@ static int create_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr
         /* Note that numblocks is 2 * (real numblocks) here;
          * see i += 2 below as it makses shifting and masking less painful
          */
-        numblocks = mtd->size >> (bbm->bbt_erase_shift - 1);
+        numblocks = this->chipsize >> (bbm->bbt_erase_shift - 1);
         startblock = 0;
         from = 0;
 
@@ -106,7 +107,12 @@ static int create_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr
                         }
                 }
                 i += 2;
-                from += (1 << bbm->bbt_erase_shift);
+
+                if (FLEXONENAND(this)) {
+                        rgn = flexonenand_region(mtd, from);
+                        from += mtd->eraseregions[rgn].erasesize;
+                } else
+                        from += (1 << bbm->bbt_erase_shift);
         }
 
         return 0;
@@ -143,7 +149,7 @@ static int onenand_isbad_bbt(struct mtd_info *mtd, loff_t offs, int allowbbt)
         uint8_t res;
 
         /* Get block number * 2 */
-        block = (int) (offs >> (bbm->bbt_erase_shift - 1));
+        block = (int) (onenand_block(this, offs) << 1);
         res = (bbm->bbt[block >> 3] >> (block & 0x06)) & 0x03;
 
         DEBUG(MTD_DEBUG_LEVEL2, "onenand_isbad_bbt: bbt info for offs 0x%08x: (block %d) 0x%02x\n",
@@ -178,7 +184,7 @@ int onenand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
         struct bbm_info *bbm = this->bbm;
         int len, ret = 0;
 
-        len = mtd->size >> (this->erase_shift + 2);
+        len = this->chipsize >> (this->erase_shift + 2);
         /* Allocate memory (2bit per block) and clear the memory bad block table */
         bbm->bbt = kzalloc(len, GFP_KERNEL);
         if (!bbm->bbt) {
diff --git a/drivers/mtd/onenand/onenand_sim.c b/drivers/mtd/onenand/onenand_sim.c
index d64200b7c94b..f6e3c8aebd3a 100644
--- a/drivers/mtd/onenand/onenand_sim.c
+++ b/drivers/mtd/onenand/onenand_sim.c
@@ -6,6 +6,10 @@
  *  Copyright © 2005-2007 Samsung Electronics
  *  Kyungmin Park <kyungmin.park@samsung.com>
  *
+ *  Vishak G <vishak.g at samsung.com>, Rohit Hagargundgi <h.rohit at samsung.com>
+ *  Flex-OneNAND simulator support
+ *  Copyright (C) Samsung Electronics, 2008
+ *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
@@ -24,16 +28,38 @@
 #ifndef CONFIG_ONENAND_SIM_MANUFACTURER
 #define CONFIG_ONENAND_SIM_MANUFACTURER         0xec
 #endif
+
 #ifndef CONFIG_ONENAND_SIM_DEVICE_ID
 #define CONFIG_ONENAND_SIM_DEVICE_ID            0x04
 #endif
+
+#define CONFIG_FLEXONENAND ((CONFIG_ONENAND_SIM_DEVICE_ID >> 9) & 1)
+
 #ifndef CONFIG_ONENAND_SIM_VERSION_ID
 #define CONFIG_ONENAND_SIM_VERSION_ID           0x1e
 #endif
 
+#ifndef CONFIG_ONENAND_SIM_TECHNOLOGY_ID
+#define CONFIG_ONENAND_SIM_TECHNOLOGY_ID CONFIG_FLEXONENAND
+#endif
+
+/* Initial boundary values for Flex-OneNAND Simulator */
+#ifndef CONFIG_FLEXONENAND_SIM_DIE0_BOUNDARY
+#define CONFIG_FLEXONENAND_SIM_DIE0_BOUNDARY    0x01
+#endif
+
+#ifndef CONFIG_FLEXONENAND_SIM_DIE1_BOUNDARY
+#define CONFIG_FLEXONENAND_SIM_DIE1_BOUNDARY    0x01
+#endif
+
 static int manuf_id     = CONFIG_ONENAND_SIM_MANUFACTURER;
 static int device_id    = CONFIG_ONENAND_SIM_DEVICE_ID;
 static int version_id   = CONFIG_ONENAND_SIM_VERSION_ID;
+static int technology_id = CONFIG_ONENAND_SIM_TECHNOLOGY_ID;
+static int boundary[] = {
+        CONFIG_FLEXONENAND_SIM_DIE0_BOUNDARY,
+        CONFIG_FLEXONENAND_SIM_DIE1_BOUNDARY,
+};
 
 struct onenand_flash {
         void __iomem *base;
@@ -57,12 +83,18 @@ struct onenand_flash {
         (writew(v, this->base + ONENAND_REG_WP_STATUS))
 
 /* It has all 0xff chars */
-#define MAX_ONENAND_PAGESIZE            (2048 + 64)
+#define MAX_ONENAND_PAGESIZE            (4096 + 128)
 static unsigned char *ffchars;
 
+#if CONFIG_FLEXONENAND
+#define PARTITION_NAME "Flex-OneNAND simulator partition"
+#else
+#define PARTITION_NAME "OneNAND simulator partition"
+#endif
+
 static struct mtd_partition os_partitions[] = {
         {
-                .name           = "OneNAND simulator partition",
+                .name           = PARTITION_NAME,
                 .offset         = 0,
                 .size           = MTDPART_SIZ_FULL,
         },
@@ -104,6 +136,7 @@ static void onenand_lock_handle(struct onenand_chip *this, int cmd)
 
         switch (cmd) {
         case ONENAND_CMD_UNLOCK:
+        case ONENAND_CMD_UNLOCK_ALL:
                 if (block_lock_scheme)
                         ONENAND_SET_WP_STATUS(ONENAND_WP_US, this);
                 else
@@ -228,10 +261,12 @@ static void onenand_data_handle(struct onenand_chip *this, int cmd,
 {
         struct mtd_info *mtd = &info->mtd;
         struct onenand_flash *flash = this->priv;
-        int main_offset, spare_offset;
+        int main_offset, spare_offset, die = 0;
         void __iomem *src;
         void __iomem *dest;
         unsigned int i;
+        static int pi_operation;
+        int erasesize, rgn;
 
         if (dataram) {
                 main_offset = mtd->writesize;
@@ -241,10 +276,27 @@ static void onenand_data_handle(struct onenand_chip *this, int cmd,
                 spare_offset = 0;
         }
 
+        if (pi_operation) {
+                die = readw(this->base + ONENAND_REG_START_ADDRESS2);
+                die >>= ONENAND_DDP_SHIFT;
+        }
+
         switch (cmd) {
+        case FLEXONENAND_CMD_PI_ACCESS:
+                pi_operation = 1;
+                break;
+
+        case ONENAND_CMD_RESET:
+                pi_operation = 0;
+                break;
+
         case ONENAND_CMD_READ:
                 src = ONENAND_CORE(flash) + offset;
                 dest = ONENAND_MAIN_AREA(this, main_offset);
+                if (pi_operation) {
+                        writew(boundary[die], this->base + ONENAND_DATARAM);
+                        break;
+                }
                 memcpy(dest, src, mtd->writesize);
                 /* Fall through */
 
@@ -257,6 +309,10 @@ static void onenand_data_handle(struct onenand_chip *this, int cmd,
         case ONENAND_CMD_PROG:
                 src = ONENAND_MAIN_AREA(this, main_offset);
                 dest = ONENAND_CORE(flash) + offset;
+                if (pi_operation) {
+                        boundary[die] = readw(this->base + ONENAND_DATARAM);
+                        break;
+                }
                 /* To handle partial write */
                 for (i = 0; i < (1 << mtd->subpage_sft); i++) {
                         int off = i * this->subpagesize;
@@ -284,9 +340,18 @@ static void onenand_data_handle(struct onenand_chip *this, int cmd,
                 break;
 
         case ONENAND_CMD_ERASE:
-                memset(ONENAND_CORE(flash) + offset, 0xff, mtd->erasesize);
+                if (pi_operation)
+                        break;
+
+                if (FLEXONENAND(this)) {
+                        rgn = flexonenand_region(mtd, offset);
+                        erasesize = mtd->eraseregions[rgn].erasesize;
+                } else
+                        erasesize = mtd->erasesize;
+
+                memset(ONENAND_CORE(flash) + offset, 0xff, erasesize);
                 memset(ONENAND_CORE_SPARE(flash, this, offset), 0xff,
-                       (mtd->erasesize >> 5));
+                       (erasesize >> 5));
                 break;
 
         default:
@@ -339,7 +404,7 @@ static void onenand_command_handle(struct onenand_chip *this, int cmd)
         }
 
         if (block != -1)
-                offset += block << this->erase_shift;
+                offset = onenand_addr(this, block);
 
         if (page != -1)
                 offset += page << this->page_shift;
@@ -390,6 +455,7 @@ static int __init flash_init(struct onenand_flash *flash)
         }
 
         density = device_id >> ONENAND_DEVICE_DENSITY_SHIFT;
+        density &= ONENAND_DEVICE_DENSITY_MASK;
         size = ((16 << 20) << density);
 
         ONENAND_CORE(flash) = vmalloc(size + (size >> 5));
@@ -405,8 +471,9 @@ static int __init flash_init(struct onenand_flash *flash)
         writew(manuf_id, flash->base + ONENAND_REG_MANUFACTURER_ID);
         writew(device_id, flash->base + ONENAND_REG_DEVICE_ID);
         writew(version_id, flash->base + ONENAND_REG_VERSION_ID);
+        writew(technology_id, flash->base + ONENAND_REG_TECHNOLOGY);
 
-        if (density < 2)
+        if (density < 2 && (!CONFIG_FLEXONENAND))
                 buffer_size = 0x0400;   /* 1KiB page */
         else
                 buffer_size = 0x0800;   /* 2KiB page */
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index 892a9e4e275f..1dc721517e4c 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -2443,6 +2443,17 @@ config JME
           To compile this driver as a module, choose M here. The module
           will be called jme.
 
+config S6GMAC
+        tristate "S6105 GMAC ethernet support"
+        depends on XTENSA_VARIANT_S6000
+        select PHYLIB
+        help
+          This driver supports the on chip ethernet device on the
+          S6105 xtensa processor.
+
+          To compile this driver as a module, choose M here. The module
+          will be called s6gmac.
+
 endif # NETDEV_1000
 
 #
diff --git a/drivers/net/Makefile b/drivers/net/Makefile
index d366fb2b40e9..4b58a59f211b 100644
--- a/drivers/net/Makefile
+++ b/drivers/net/Makefile
@@ -245,6 +245,7 @@ obj-$(CONFIG_XTENSA_XT2000_SONIC) += xtsonic.o
 
 obj-$(CONFIG_DNET) += dnet.o
 obj-$(CONFIG_MACB) += macb.o
+obj-$(CONFIG_S6GMAC) += s6gmac.o
 
 obj-$(CONFIG_ARM) += arm/
 obj-$(CONFIG_DEV_APPLETALK) += appletalk/
diff --git a/drivers/net/benet/be.h b/drivers/net/benet/be.h
index b4bb06fdf307..f703758f0a6e 100644
--- a/drivers/net/benet/be.h
+++ b/drivers/net/benet/be.h
@@ -65,7 +65,7 @@ static inline char *nic_name(struct pci_dev *pdev)
 #define TX_CQ_LEN               1024
 #define RX_Q_LEN                1024    /* Does not support any other value */
 #define RX_CQ_LEN               1024
-#define MCC_Q_LEN               64      /* total size not to exceed 8 pages */
+#define MCC_Q_LEN               128     /* total size not to exceed 8 pages */
 #define MCC_CQ_LEN              256
 
 #define BE_NAPI_WEIGHT          64
@@ -91,6 +91,61 @@ struct be_queue_info {
         atomic_t used;  /* Number of valid elements in the queue */
 };
 
+static inline u32 MODULO(u16 val, u16 limit)
+{
+        BUG_ON(limit & (limit - 1));
+        return val & (limit - 1);
+}
+
+static inline void index_adv(u16 *index, u16 val, u16 limit)
+{
+        *index = MODULO((*index + val), limit);
+}
+
+static inline void index_inc(u16 *index, u16 limit)
+{
+        *index = MODULO((*index + 1), limit);
+}
+
+static inline void *queue_head_node(struct be_queue_info *q)
+{
+        return q->dma_mem.va + q->head * q->entry_size;
+}
+
+static inline void *queue_tail_node(struct be_queue_info *q)
+{
+        return q->dma_mem.va + q->tail * q->entry_size;
+}
+
+static inline void queue_head_inc(struct be_queue_info *q)
+{
+        index_inc(&q->head, q->len);
+}
+
+static inline void queue_tail_inc(struct be_queue_info *q)
+{
+        index_inc(&q->tail, q->len);
+}
+
+
+struct be_eq_obj {
+        struct be_queue_info q;
+        char desc[32];
+
+        /* Adaptive interrupt coalescing (AIC) info */
+        bool enable_aic;
+        u16 min_eqd;            /* in usecs */
+        u16 max_eqd;            /* in usecs */
+        u16 cur_eqd;            /* in usecs */
+
+        struct napi_struct napi;
+};
+
+struct be_mcc_obj {
+        struct be_queue_info q;
+        struct be_queue_info cq;
+};
+
 struct be_ctrl_info {
         u8 __iomem *csr;
         u8 __iomem *db;         /* Door Bell */
@@ -98,11 +153,20 @@ struct be_ctrl_info {
         int pci_func;
 
         /* Mbox used for cmd request/response */
-        spinlock_t cmd_lock;    /* For serializing cmds to BE card */
+        spinlock_t mbox_lock;   /* For serializing mbox cmds to BE card */
         struct be_dma_mem mbox_mem;
         /* Mbox mem is adjusted to align to 16 bytes. The allocated addr
          * is stored for freeing purpose */
         struct be_dma_mem mbox_mem_alloced;
+
+        /* MCC Rings */
+        struct be_mcc_obj mcc_obj;
+        spinlock_t mcc_lock;    /* For serializing mcc cmds to BE card */
+        spinlock_t mcc_cq_lock;
+
+        /* MCC Async callback */
+        void (*async_cb)(void *adapter, bool link_up);
+        void *adapter_ctxt;
 };
 
 #include "be_cmds.h"
@@ -150,19 +214,6 @@ struct be_stats_obj {
         struct be_dma_mem cmd;
 };
 
-struct be_eq_obj {
-        struct be_queue_info q;
-        char desc[32];
-
-        /* Adaptive interrupt coalescing (AIC) info */
-        bool enable_aic;
-        u16 min_eqd;            /* in usecs */
-        u16 max_eqd;            /* in usecs */
-        u16 cur_eqd;            /* in usecs */
-
-        struct napi_struct napi;
-};
-
 struct be_tx_obj {
         struct be_queue_info q;
         struct be_queue_info cq;
@@ -225,8 +276,9 @@ struct be_adapter {
         u32 if_handle;          /* Used to configure filtering */
         u32 pmac_id;            /* MAC addr handle used by BE card */
 
-        struct be_link_info link;
+        bool link_up;
         u32 port_num;
+        bool promiscuous;
 };
 
 extern struct ethtool_ops be_ethtool_ops;
@@ -235,22 +287,6 @@ extern struct ethtool_ops be_ethtool_ops;
 
 #define BE_SET_NETDEV_OPS(netdev, ops)  (netdev->netdev_ops = ops)
 
-static inline u32 MODULO(u16 val, u16 limit)
-{
-        BUG_ON(limit & (limit - 1));
-        return val & (limit - 1);
-}
-
-static inline void index_adv(u16 *index, u16 val, u16 limit)
-{
-        *index = MODULO((*index + val), limit);
-}
-
-static inline void index_inc(u16 *index, u16 limit)
-{
-        *index = MODULO((*index + 1), limit);
-}
-
 #define PAGE_SHIFT_4K           12
 #define PAGE_SIZE_4K            (1 << PAGE_SHIFT_4K)
 
@@ -339,4 +375,6 @@ static inline u8 is_udp_pkt(struct sk_buff *skb)
         return val;
 }
 
+extern void be_cq_notify(struct be_ctrl_info *ctrl, u16 qid, bool arm,
+                u16 num_popped);
 #endif                          /* BE_H */
diff --git a/drivers/net/benet/be_cmds.c b/drivers/net/benet/be_cmds.c
index d444aed962bc..583517ed56f0 100644
--- a/drivers/net/benet/be_cmds.c
+++ b/drivers/net/benet/be_cmds.c
@@ -17,6 +17,133 @@
 
 #include "be.h"
 
+static void be_mcc_notify(struct be_ctrl_info *ctrl)
+{
+        struct be_queue_info *mccq = &ctrl->mcc_obj.q;
+        u32 val = 0;
+
+        val |= mccq->id & DB_MCCQ_RING_ID_MASK;
+        val |= 1 << DB_MCCQ_NUM_POSTED_SHIFT;
+        iowrite32(val, ctrl->db + DB_MCCQ_OFFSET);
+}
+
+/* To check if valid bit is set, check the entire word as we don't know
+ * the endianness of the data (old entry is host endian while a new entry is
+ * little endian) */
+static inline bool be_mcc_compl_is_new(struct be_mcc_cq_entry *compl)
+{
+        if (compl->flags != 0) {
+                compl->flags = le32_to_cpu(compl->flags);
+                BUG_ON((compl->flags & CQE_FLAGS_VALID_MASK) == 0);
+                return true;
+        } else {
+                return false;
+        }
+}
+
+/* Need to reset the entire word that houses the valid bit */
+static inline void be_mcc_compl_use(struct be_mcc_cq_entry *compl)
+{
+        compl->flags = 0;
+}
+
+static int be_mcc_compl_process(struct be_ctrl_info *ctrl,
+        struct be_mcc_cq_entry *compl)
+{
+        u16 compl_status, extd_status;
+
+        /* Just swap the status to host endian; mcc tag is opaquely copied
+         * from mcc_wrb */
+        be_dws_le_to_cpu(compl, 4);
+
+        compl_status = (compl->status >> CQE_STATUS_COMPL_SHIFT) &
+                                CQE_STATUS_COMPL_MASK;
+        if (compl_status != MCC_STATUS_SUCCESS) {
+                extd_status = (compl->status >> CQE_STATUS_EXTD_SHIFT) &
+                                CQE_STATUS_EXTD_MASK;
+                printk(KERN_WARNING DRV_NAME
+                        " error in cmd completion: status(compl/extd)=%d/%d\n",
+                        compl_status, extd_status);
+                return -1;
+        }
+        return 0;
+}
+
+/* Link state evt is a string of bytes; no need for endian swapping */
+static void be_async_link_state_process(struct be_ctrl_info *ctrl,
+                struct be_async_event_link_state *evt)
+{
+        ctrl->async_cb(ctrl->adapter_ctxt,
+                evt->port_link_status == ASYNC_EVENT_LINK_UP ? true : false);
+}
+
+static inline bool is_link_state_evt(u32 trailer)
+{
+        return (((trailer >> ASYNC_TRAILER_EVENT_CODE_SHIFT) &
+                ASYNC_TRAILER_EVENT_CODE_MASK) ==
+                                ASYNC_EVENT_CODE_LINK_STATE);
+}
+
+static struct be_mcc_cq_entry *be_mcc_compl_get(struct be_ctrl_info *ctrl)
+{
+        struct be_queue_info *mcc_cq = &ctrl->mcc_obj.cq;
+        struct be_mcc_cq_entry *compl = queue_tail_node(mcc_cq);
+
+        if (be_mcc_compl_is_new(compl)) {
+                queue_tail_inc(mcc_cq);
+                return compl;
+        }
+        return NULL;
+}
+
+void be_process_mcc(struct be_ctrl_info *ctrl)
+{
+        struct be_mcc_cq_entry *compl;
+        int num = 0;
+
+        spin_lock_bh(&ctrl->mcc_cq_lock);
+        while ((compl = be_mcc_compl_get(ctrl))) {
+                if (compl->flags & CQE_FLAGS_ASYNC_MASK) {
+                        /* Interpret flags as an async trailer */
+                        BUG_ON(!is_link_state_evt(compl->flags));
+
+                        /* Interpret compl as a async link evt */
+                        be_async_link_state_process(ctrl,
+                                (struct be_async_event_link_state *) compl);
+                } else {
+                        be_mcc_compl_process(ctrl, compl);
+                        atomic_dec(&ctrl->mcc_obj.q.used);
+                }
+                be_mcc_compl_use(compl);
+                num++;
+        }
+        if (num)
+                be_cq_notify(ctrl, ctrl->mcc_obj.cq.id, true, num);
+        spin_unlock_bh(&ctrl->mcc_cq_lock);
+}
+
+/* Wait till no more pending mcc requests are present */
+static void be_mcc_wait_compl(struct be_ctrl_info *ctrl)
+{
+#define mcc_timeout             50000 /* 5s timeout */
+        int i;
+        for (i = 0; i < mcc_timeout; i++) {
+                be_process_mcc(ctrl);
+                if (atomic_read(&ctrl->mcc_obj.q.used) == 0)
+                        break;
+                udelay(100);
+        }
+        if (i == mcc_timeout)
+                printk(KERN_WARNING DRV_NAME "mcc poll timed out\n");
+}
+
+/* Notify MCC requests and wait for completion */
+static void be_mcc_notify_wait(struct be_ctrl_info *ctrl)
+{
+        be_mcc_notify(ctrl);
+        be_mcc_wait_compl(ctrl);
+}
+
 static int be_mbox_db_ready_wait(void __iomem *db)
 {
         int cnt = 0, wait = 5;
@@ -44,11 +171,11 @@ static int be_mbox_db_ready_wait(void __iomem *db)
 
 /*
  * Insert the mailbox address into the doorbell in two steps
+ * Polls on the mbox doorbell till a command completion (or a timeout) occurs
  */
 static int be_mbox_db_ring(struct be_ctrl_info *ctrl)
 {
         int status;
-        u16 compl_status, extd_status;
         u32 val = 0;
         void __iomem *db = ctrl->db + MPU_MAILBOX_DB_OFFSET;
         struct be_dma_mem *mbox_mem = &ctrl->mbox_mem;
@@ -79,24 +206,17 @@ static int be_mbox_db_ring(struct be_ctrl_info *ctrl)
         if (status != 0)
                 return status;
 
-        /* compl entry has been made now */
-        be_dws_le_to_cpu(cqe, sizeof(*cqe));
-        if (!(cqe->flags & CQE_FLAGS_VALID_MASK)) {
-                printk(KERN_WARNING DRV_NAME ": ERROR invalid mbox compl\n");
+        /* A cq entry has been made now */
+        if (be_mcc_compl_is_new(cqe)) {
+                status = be_mcc_compl_process(ctrl, &mbox->cqe);
+                be_mcc_compl_use(cqe);
+                if (status)
+                        return status;
+        } else {
+                printk(KERN_WARNING DRV_NAME "invalid mailbox completion\n");
                 return -1;
         }
-
-        compl_status = (cqe->status >> CQE_STATUS_COMPL_SHIFT) &
-                                CQE_STATUS_COMPL_MASK;
-        if (compl_status != MCC_STATUS_SUCCESS) {
-                extd_status = (cqe->status >> CQE_STATUS_EXTD_SHIFT) &
-                                CQE_STATUS_EXTD_MASK;
-                printk(KERN_WARNING DRV_NAME
-                        ": ERROR in cmd compl. status(compl/extd)=%d/%d\n",
-                        compl_status, extd_status);
-        }
-
-        return compl_status;
+        return 0;
 }
 
 static int be_POST_stage_get(struct be_ctrl_info *ctrl, u16 *stage)
@@ -235,6 +355,18 @@ static inline struct be_mcc_wrb *wrb_from_mbox(struct be_dma_mem *mbox_mem)
         return &((struct be_mcc_mailbox *)(mbox_mem->va))->wrb;
 }
 
+static inline struct be_mcc_wrb *wrb_from_mcc(struct be_queue_info *mccq)
+{
+        struct be_mcc_wrb *wrb = NULL;
+        if (atomic_read(&mccq->used) < mccq->len) {
+                wrb = queue_head_node(mccq);
+                queue_head_inc(mccq);
+                atomic_inc(&mccq->used);
+                memset(wrb, 0, sizeof(*wrb));
+        }
+        return wrb;
+}
+
 int be_cmd_eq_create(struct be_ctrl_info *ctrl,
                 struct be_queue_info *eq, int eq_delay)
 {
@@ -244,7 +376,7 @@ int be_cmd_eq_create(struct be_ctrl_info *ctrl,
         struct be_dma_mem *q_mem = &eq->dma_mem;
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -272,7 +404,7 @@ int be_cmd_eq_create(struct be_ctrl_info *ctrl,
                 eq->id = le16_to_cpu(resp->eq_id);
                 eq->created = true;
         }
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
         return status;
 }
 
@@ -284,7 +416,7 @@ int be_cmd_mac_addr_query(struct be_ctrl_info *ctrl, u8 *mac_addr,
         struct be_cmd_resp_mac_query *resp = embedded_payload(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -304,7 +436,7 @@ int be_cmd_mac_addr_query(struct be_ctrl_info *ctrl, u8 *mac_addr,
         if (!status)
                 memcpy(mac_addr, resp->mac.addr, ETH_ALEN);
 
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
         return status;
 }
 
@@ -315,7 +447,7 @@ int be_cmd_pmac_add(struct be_ctrl_info *ctrl, u8 *mac_addr,
         struct be_cmd_req_pmac_add *req = embedded_payload(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -332,7 +464,7 @@ int be_cmd_pmac_add(struct be_ctrl_info *ctrl, u8 *mac_addr,
                 *pmac_id = le32_to_cpu(resp->pmac_id);
         }
 
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
         return status;
 }
 
@@ -342,7 +474,7 @@ int be_cmd_pmac_del(struct be_ctrl_info *ctrl, u32 if_id, u32 pmac_id)
         struct be_cmd_req_pmac_del *req = embedded_payload(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -354,7 +486,7 @@ int be_cmd_pmac_del(struct be_ctrl_info *ctrl, u32 if_id, u32 pmac_id)
         req->pmac_id = cpu_to_le32(pmac_id);
 
         status = be_mbox_db_ring(ctrl);
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
 
         return status;
 }
@@ -370,7 +502,7 @@ int be_cmd_cq_create(struct be_ctrl_info *ctrl,
         void *ctxt = &req->context;
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -388,7 +520,7 @@ int be_cmd_cq_create(struct be_ctrl_info *ctrl,
         AMAP_SET_BITS(struct amap_cq_context, solevent, ctxt, sol_evts);
         AMAP_SET_BITS(struct amap_cq_context, eventable, ctxt, 1);
         AMAP_SET_BITS(struct amap_cq_context, eqid, ctxt, eq->id);
-        AMAP_SET_BITS(struct amap_cq_context, armed, ctxt, 0);
+        AMAP_SET_BITS(struct amap_cq_context, armed, ctxt, 1);
         AMAP_SET_BITS(struct amap_cq_context, func, ctxt, ctrl->pci_func);
         be_dws_cpu_to_le(ctxt, sizeof(req->context));
 
@@ -399,7 +531,56 @@ int be_cmd_cq_create(struct be_ctrl_info *ctrl,
                 cq->id = le16_to_cpu(resp->cq_id);
                 cq->created = true;
         }
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
+
+        return status;
+}
+
+static u32 be_encoded_q_len(int q_len)
+{
+        u32 len_encoded = fls(q_len); /* log2(len) + 1 */
+        if (len_encoded == 16)
+                len_encoded = 0;
+        return len_encoded;
+}
+
+int be_cmd_mccq_create(struct be_ctrl_info *ctrl,
+                        struct be_queue_info *mccq,
+                        struct be_queue_info *cq)
+{
+        struct be_mcc_wrb *wrb = wrb_from_mbox(&ctrl->mbox_mem);
+        struct be_cmd_req_mcc_create *req = embedded_payload(wrb);
+        struct be_dma_mem *q_mem = &mccq->dma_mem;
+        void *ctxt = &req->context;
+        int status;
+
+        spin_lock(&ctrl->mbox_lock);
+        memset(wrb, 0, sizeof(*wrb));
+
+        be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+
+        be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+                        OPCODE_COMMON_MCC_CREATE, sizeof(*req));
+
+        req->num_pages = PAGES_4K_SPANNED(q_mem->va, q_mem->size);
+
+        AMAP_SET_BITS(struct amap_mcc_context, fid, ctxt, ctrl->pci_func);
+        AMAP_SET_BITS(struct amap_mcc_context, valid, ctxt, 1);
+        AMAP_SET_BITS(struct amap_mcc_context, ring_size, ctxt,
+                be_encoded_q_len(mccq->len));
+        AMAP_SET_BITS(struct amap_mcc_context, cq_id, ctxt, cq->id);
+
+        be_dws_cpu_to_le(ctxt, sizeof(req->context));
+
+        be_cmd_page_addrs_prepare(req->pages, ARRAY_SIZE(req->pages), q_mem);
+
+        status = be_mbox_db_ring(ctrl);
+        if (!status) {
+                struct be_cmd_resp_mcc_create *resp = embedded_payload(wrb);
+                mccq->id = le16_to_cpu(resp->id);
+                mccq->created = true;
+        }
+        spin_unlock(&ctrl->mbox_lock);
 
         return status;
 }
@@ -415,7 +596,7 @@ int be_cmd_txq_create(struct be_ctrl_info *ctrl,
         int status;
         u32 len_encoded;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -446,7 +627,7 @@ int be_cmd_txq_create(struct be_ctrl_info *ctrl,
                 txq->id = le16_to_cpu(resp->cid);
                 txq->created = true;
         }
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
 
         return status;
 }
@@ -460,7 +641,7 @@ int be_cmd_rxq_create(struct be_ctrl_info *ctrl,
         struct be_dma_mem *q_mem = &rxq->dma_mem;
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -482,7 +663,7 @@ int be_cmd_rxq_create(struct be_ctrl_info *ctrl,
                 rxq->id = le16_to_cpu(resp->id);
                 rxq->created = true;
         }
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
 
         return status;
 }
@@ -496,7 +677,7 @@ int be_cmd_q_destroy(struct be_ctrl_info *ctrl, struct be_queue_info *q,
         u8 subsys = 0, opcode = 0;
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
 
         memset(wrb, 0, sizeof(*wrb));
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -518,6 +699,10 @@ int be_cmd_q_destroy(struct be_ctrl_info *ctrl, struct be_queue_info *q,
                 subsys = CMD_SUBSYSTEM_ETH;
                 opcode = OPCODE_ETH_RX_DESTROY;
                 break;
+        case QTYPE_MCCQ:
+                subsys = CMD_SUBSYSTEM_COMMON;
+                opcode = OPCODE_COMMON_MCC_DESTROY;
+                break;
         default:
                 printk(KERN_WARNING DRV_NAME ":bad Q type in Q destroy cmd\n");
                 status = -1;
@@ -528,7 +713,7 @@ int be_cmd_q_destroy(struct be_ctrl_info *ctrl, struct be_queue_info *q,
 
         status = be_mbox_db_ring(ctrl);
 err:
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
 
         return status;
 }
@@ -541,7 +726,7 @@ int be_cmd_if_create(struct be_ctrl_info *ctrl, u32 flags, u8 *mac,
         struct be_cmd_req_if_create *req = embedded_payload(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -562,7 +747,7 @@ int be_cmd_if_create(struct be_ctrl_info *ctrl, u32 flags, u8 *mac,
                         *pmac_id = le32_to_cpu(resp->pmac_id);
         }
 
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
         return status;
 }
 
@@ -572,7 +757,7 @@ int be_cmd_if_destroy(struct be_ctrl_info *ctrl, u32 interface_id)
         struct be_cmd_req_if_destroy *req = embedded_payload(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -583,7 +768,7 @@ int be_cmd_if_destroy(struct be_ctrl_info *ctrl, u32 interface_id)
         req->interface_id = cpu_to_le32(interface_id);
         status = be_mbox_db_ring(ctrl);
 
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
 
         return status;
 }
@@ -598,7 +783,7 @@ int be_cmd_get_stats(struct be_ctrl_info *ctrl, struct be_dma_mem *nonemb_cmd)
         struct be_sge *sge = nonembedded_sgl(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         memset(req, 0, sizeof(*req));
@@ -617,18 +802,20 @@ int be_cmd_get_stats(struct be_ctrl_info *ctrl, struct be_dma_mem *nonemb_cmd)
                 be_dws_le_to_cpu(&resp->hw_stats, sizeof(resp->hw_stats));
         }
 
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
         return status;
 }
 
 int be_cmd_link_status_query(struct be_ctrl_info *ctrl,
-                        struct be_link_info *link)
+                        bool *link_up)
 {
         struct be_mcc_wrb *wrb = wrb_from_mbox(&ctrl->mbox_mem);
         struct be_cmd_req_link_status *req = embedded_payload(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
+
+        *link_up = false;
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -639,14 +826,11 @@ int be_cmd_link_status_query(struct be_ctrl_info *ctrl,
         status = be_mbox_db_ring(ctrl);
         if (!status) {
                 struct be_cmd_resp_link_status *resp = embedded_payload(wrb);
-                link->speed = resp->mac_speed;
-                link->duplex = resp->mac_duplex;
-                link->fault = resp->mac_fault;
-        } else {
-                link->speed = PHY_LINK_SPEED_ZERO;
+                if (resp->mac_speed != PHY_LINK_SPEED_ZERO)
+                        *link_up = true;
         }
 
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
         return status;
 }
 
@@ -656,7 +840,7 @@ int be_cmd_get_fw_ver(struct be_ctrl_info *ctrl, char *fw_ver)
         struct be_cmd_req_get_fw_version *req = embedded_payload(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -670,7 +854,7 @@ int be_cmd_get_fw_ver(struct be_ctrl_info *ctrl, char *fw_ver)
                 strncpy(fw_ver, resp->firmware_version_string, FW_VER_LEN);
         }
 
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
         return status;
 }
 
@@ -681,7 +865,7 @@ int be_cmd_modify_eqd(struct be_ctrl_info *ctrl, u32 eq_id, u32 eqd)
         struct be_cmd_req_modify_eq_delay *req = embedded_payload(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -696,7 +880,7 @@ int be_cmd_modify_eqd(struct be_ctrl_info *ctrl, u32 eq_id, u32 eqd)
 
         status = be_mbox_db_ring(ctrl);
 
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
         return status;
 }
 
@@ -707,7 +891,7 @@ int be_cmd_vlan_config(struct be_ctrl_info *ctrl, u32 if_id, u16 *vtag_array,
         struct be_cmd_req_vlan_config *req = embedded_payload(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
         memset(wrb, 0, sizeof(*wrb));
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
@@ -726,18 +910,22 @@ int be_cmd_vlan_config(struct be_ctrl_info *ctrl, u32 if_id, u16 *vtag_array,
 
         status = be_mbox_db_ring(ctrl);
 
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
         return status;
 }
 
+/* Use MCC for this command as it may be called in BH context */
 int be_cmd_promiscuous_config(struct be_ctrl_info *ctrl, u8 port_num, bool en)
 {
-        struct be_mcc_wrb *wrb = wrb_from_mbox(&ctrl->mbox_mem);
-        struct be_cmd_req_promiscuous_config *req = embedded_payload(wrb);
-        int status;
+        struct be_mcc_wrb *wrb;
+        struct be_cmd_req_promiscuous_config *req;
 
-        spin_lock(&ctrl->cmd_lock);
-        memset(wrb, 0, sizeof(*wrb));
+        spin_lock_bh(&ctrl->mcc_lock);
+
+        wrb = wrb_from_mcc(&ctrl->mcc_obj.q);
+        BUG_ON(!wrb);
+
+        req = embedded_payload(wrb);
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
 
@@ -749,21 +937,29 @@ int be_cmd_promiscuous_config(struct be_ctrl_info *ctrl, u8 port_num, bool en)
         else
                 req->port0_promiscuous = en;
 
-        status = be_mbox_db_ring(ctrl);
+        be_mcc_notify_wait(ctrl);
 
-        spin_unlock(&ctrl->cmd_lock);
-        return status;
+        spin_unlock_bh(&ctrl->mcc_lock);
+        return 0;
 }
 
-int be_cmd_mcast_mac_set(struct be_ctrl_info *ctrl, u32 if_id, u8 *mac_table,
-                        u32 num, bool promiscuous)
+/*
+ * Use MCC for this command as it may be called in BH context
+ * (mc == NULL) => multicast promiscous
+ */
+int be_cmd_multicast_set(struct be_ctrl_info *ctrl, u32 if_id,
+                struct dev_mc_list *mc_list, u32 mc_count)
 {
-        struct be_mcc_wrb *wrb = wrb_from_mbox(&ctrl->mbox_mem);
-        struct be_cmd_req_mcast_mac_config *req = embedded_payload(wrb);
-        int status;
+#define BE_MAX_MC               32 /* set mcast promisc if > 32 */
+        struct be_mcc_wrb *wrb;
+        struct be_cmd_req_mcast_mac_config *req;
 
-        spin_lock(&ctrl->cmd_lock);
-        memset(wrb, 0, sizeof(*wrb));
+        spin_lock_bh(&ctrl->mcc_lock);
+
+        wrb = wrb_from_mcc(&ctrl->mcc_obj.q);
+        BUG_ON(!wrb);
+
+        req = embedded_payload(wrb);
 
         be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
 
@@ -771,17 +967,23 @@ int be_cmd_mcast_mac_set(struct be_ctrl_info *ctrl, u32 if_id, u8 *mac_table,
                 OPCODE_COMMON_NTWK_MULTICAST_SET, sizeof(*req));
 
         req->interface_id = if_id;
-        req->promiscuous = promiscuous;
-        if (!promiscuous) {
-                req->num_mac = cpu_to_le16(num);
-                if (num)
-                        memcpy(req->mac, mac_table, ETH_ALEN * num);
+        if (mc_list && mc_count <= BE_MAX_MC) {
+                int i;
+                struct dev_mc_list *mc;
+
+                req->num_mac = cpu_to_le16(mc_count);
+
+                for (mc = mc_list, i = 0; mc; mc = mc->next, i++)
+                        memcpy(req->mac[i].byte, mc->dmi_addr, ETH_ALEN);
+        } else {
+                req->promiscuous = 1;
         }
 
-        status = be_mbox_db_ring(ctrl);
+        be_mcc_notify_wait(ctrl);
 
-        spin_unlock(&ctrl->cmd_lock);
-        return status;
+        spin_unlock_bh(&ctrl->mcc_lock);
+
+        return 0;
 }
 
 int be_cmd_set_flow_control(struct be_ctrl_info *ctrl, u32 tx_fc, u32 rx_fc)
@@ -790,7 +992,7 @@ int be_cmd_set_flow_control(struct be_ctrl_info *ctrl, u32 tx_fc, u32 rx_fc)
         struct be_cmd_req_set_flow_control *req = embedded_payload(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
 
         memset(wrb, 0, sizeof(*wrb));
 
@@ -804,7 +1006,7 @@ int be_cmd_set_flow_control(struct be_ctrl_info *ctrl, u32 tx_fc, u32 rx_fc)
 
         status = be_mbox_db_ring(ctrl);
 
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
         return status;
 }
 
@@ -814,7 +1016,7 @@ int be_cmd_get_flow_control(struct be_ctrl_info *ctrl, u32 *tx_fc, u32 *rx_fc)
         struct be_cmd_req_get_flow_control *req = embedded_payload(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
 
         memset(wrb, 0, sizeof(*wrb));
 
@@ -831,7 +1033,7 @@ int be_cmd_get_flow_control(struct be_ctrl_info *ctrl, u32 *tx_fc, u32 *rx_fc)
                 *rx_fc = le16_to_cpu(resp->rx_flow_control);
         }
 
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
         return status;
 }
 
@@ -841,7 +1043,7 @@ int be_cmd_query_fw_cfg(struct be_ctrl_info *ctrl, u32 *port_num)
         struct be_cmd_req_query_fw_cfg *req = embedded_payload(wrb);
         int status;
 
-        spin_lock(&ctrl->cmd_lock);
+        spin_lock(&ctrl->mbox_lock);
 
         memset(wrb, 0, sizeof(*wrb));
 
@@ -856,6 +1058,6 @@ int be_cmd_query_fw_cfg(struct be_ctrl_info *ctrl, u32 *port_num)
                 *port_num = le32_to_cpu(resp->phys_port);
         }
 
-        spin_unlock(&ctrl->cmd_lock);
+        spin_unlock(&ctrl->mbox_lock);
         return status;
 }
diff --git a/drivers/net/benet/be_cmds.h b/drivers/net/benet/be_cmds.h
index e499e2d5b8c3..747626da7b4e 100644
--- a/drivers/net/benet/be_cmds.h
+++ b/drivers/net/benet/be_cmds.h
@@ -76,6 +76,34 @@ struct be_mcc_cq_entry {
         u32 flags;              /* dword 3 */
 };
 
+/* When the async bit of mcc_compl is set, the last 4 bytes of
+ * mcc_compl is interpreted as follows:
+ */
+#define ASYNC_TRAILER_EVENT_CODE_SHIFT  8       /* bits 8 - 15 */
+#define ASYNC_TRAILER_EVENT_CODE_MASK   0xFF
+#define ASYNC_EVENT_CODE_LINK_STATE     0x1
+struct be_async_event_trailer {
+        u32 code;
+};
+
+enum {
+        ASYNC_EVENT_LINK_DOWN   = 0x0,
+        ASYNC_EVENT_LINK_UP     = 0x1
+};
+
+/* When the event code of an async trailer is link-state, the mcc_compl
+ * must be interpreted as follows
+ */
+struct be_async_event_link_state {
+        u8 physical_port;
+        u8 port_link_status;
+        u8 port_duplex;
+        u8 port_speed;
+        u8 port_fault;
+        u8 rsvd0[7];
+        struct be_async_event_trailer trailer;
+} __packed;
+
 struct be_mcc_mailbox {
         struct be_mcc_wrb wrb;
         struct be_mcc_cq_entry cqe;
@@ -101,6 +129,7 @@ struct be_mcc_mailbox {
 #define OPCODE_COMMON_FIRMWARE_CONFIG                   42
 #define OPCODE_COMMON_NTWK_INTERFACE_CREATE             50
 #define OPCODE_COMMON_NTWK_INTERFACE_DESTROY            51
+#define OPCODE_COMMON_MCC_DESTROY                       53
 #define OPCODE_COMMON_CQ_DESTROY                        54
 #define OPCODE_COMMON_EQ_DESTROY                        55
 #define OPCODE_COMMON_QUERY_FIRMWARE_CONFIG             58
@@ -269,6 +298,38 @@ struct be_cmd_resp_cq_create {
         u16 rsvd0;
 } __packed;
 
+/******************** Create MCCQ ***************************/
+/* Pseudo amap definition in which each bit of the actual structure is defined
+ * as a byte: used to calculate offset/shift/mask of each field */
+struct amap_mcc_context {
+        u8 con_index[14];
+        u8 rsvd0[2];
+        u8 ring_size[4];
+        u8 fetch_wrb;
+        u8 fetch_r2t;
+        u8 cq_id[10];
+        u8 prod_index[14];
+        u8 fid[8];
+        u8 pdid[9];
+        u8 valid;
+        u8 rsvd1[32];
+        u8 rsvd2[32];
+} __packed;
+
+struct be_cmd_req_mcc_create {
+        struct be_cmd_req_hdr hdr;
+        u16 num_pages;
+        u16 rsvd0;
+        u8 context[sizeof(struct amap_mcc_context) / 8];
+        struct phys_addr pages[8];
+} __packed;
+
+struct be_cmd_resp_mcc_create {
+        struct be_cmd_resp_hdr hdr;
+        u16 id;
+        u16 rsvd0;
+} __packed;
+
 /******************** Create TxQ ***************************/
 #define BE_ETH_TX_RING_TYPE_STANDARD            2
 #define BE_ULP1_NUM                             1
@@ -341,7 +402,8 @@ enum {
         QTYPE_EQ = 1,
         QTYPE_CQ,
         QTYPE_TXQ,
-        QTYPE_RXQ
+        QTYPE_RXQ,
+        QTYPE_MCCQ
 };
 
 struct be_cmd_req_q_destroy {
@@ -546,12 +608,6 @@ struct be_cmd_req_link_status {
         u32 rsvd;
 };
 
-struct be_link_info {
-        u8 duplex;
-        u8 speed;
-        u8 fault;
-};
-
 enum {
         PHY_LINK_DUPLEX_NONE = 0x0,
         PHY_LINK_DUPLEX_HALF = 0x1,
@@ -657,6 +713,9 @@ extern int be_cmd_cq_create(struct be_ctrl_info *ctrl,
                         struct be_queue_info *cq, struct be_queue_info *eq,
                         bool sol_evts, bool no_delay,
                         int num_cqe_dma_coalesce);
+extern int be_cmd_mccq_create(struct be_ctrl_info *ctrl,
+                        struct be_queue_info *mccq,
+                        struct be_queue_info *cq);
 extern int be_cmd_txq_create(struct be_ctrl_info *ctrl,
                         struct be_queue_info *txq,
                         struct be_queue_info *cq);
@@ -667,7 +726,7 @@ extern int be_cmd_rxq_create(struct be_ctrl_info *ctrl,
 extern int be_cmd_q_destroy(struct be_ctrl_info *ctrl, struct be_queue_info *q,
                         int type);
 extern int be_cmd_link_status_query(struct be_ctrl_info *ctrl,
-                        struct be_link_info *link);
+                        bool *link_up);
 extern int be_cmd_reset(struct be_ctrl_info *ctrl);
 extern int be_cmd_get_stats(struct be_ctrl_info *ctrl,
                         struct be_dma_mem *nonemb_cmd);
@@ -679,10 +738,11 @@ extern int be_cmd_vlan_config(struct be_ctrl_info *ctrl, u32 if_id,
                         bool promiscuous);
 extern int be_cmd_promiscuous_config(struct be_ctrl_info *ctrl,
                         u8 port_num, bool en);
-extern int be_cmd_mcast_mac_set(struct be_ctrl_info *ctrl, u32 if_id,
-                        u8 *mac_table, u32 num, bool promiscuous);
+extern int be_cmd_multicast_set(struct be_ctrl_info *ctrl, u32 if_id,
+                        struct dev_mc_list *mc_list, u32 mc_count);
 extern int be_cmd_set_flow_control(struct be_ctrl_info *ctrl,
                         u32 tx_fc, u32 rx_fc);
 extern int be_cmd_get_flow_control(struct be_ctrl_info *ctrl,
                         u32 *tx_fc, u32 *rx_fc);
 extern int be_cmd_query_fw_cfg(struct be_ctrl_info *ctrl, u32 *port_num);
+extern void be_process_mcc(struct be_ctrl_info *ctrl);
diff --git a/drivers/net/benet/be_hw.h b/drivers/net/benet/be_hw.h
index b132aa4893ca..b02e805c1db3 100644
--- a/drivers/net/benet/be_hw.h
+++ b/drivers/net/benet/be_hw.h
@@ -61,7 +61,7 @@
 /* Clear the interrupt for this eq */
 #define DB_EQ_CLR_SHIFT                 (9)     /* bit 9 */
 /* Must be 1 */
-#define DB_EQ_EVNT_SHIFT                        (10)    /* bit 10 */
+#define DB_EQ_EVNT_SHIFT                (10)    /* bit 10 */
 /* Number of event entries processed */
 #define DB_EQ_NUM_POPPED_SHIFT          (16)    /* bits 16 - 28 */
 /* Rearm bit */
@@ -88,6 +88,12 @@
 /* Number of rx frags posted */
 #define DB_RQ_NUM_POSTED_SHIFT          (24)    /* bits 24 - 31 */
 
+/********** MCC door bell ************/
+#define DB_MCCQ_OFFSET                  0x140
+#define DB_MCCQ_RING_ID_MASK            0x7FF   /* bits 0 - 10 */
+/* Number of entries posted */
+#define DB_MCCQ_NUM_POSTED_SHIFT        (16)    /* bits 16 - 29 */
+
 /*
  * BE descriptors: host memory data structures whose formats
  * are hardwired in BE silicon.
diff --git a/drivers/net/benet/be_main.c b/drivers/net/benet/be_main.c
index 66bb56874d9b..66c10c87f517 100644
--- a/drivers/net/benet/be_main.c
+++ b/drivers/net/benet/be_main.c
@@ -60,26 +60,6 @@ static int be_queue_alloc(struct be_adapter *adapter, struct be_queue_info *q,
         return 0;
 }
 
-static inline void *queue_head_node(struct be_queue_info *q)
-{
-        return q->dma_mem.va + q->head * q->entry_size;
-}
-
-static inline void *queue_tail_node(struct be_queue_info *q)
-{
-        return q->dma_mem.va + q->tail * q->entry_size;
-}
-
-static inline void queue_head_inc(struct be_queue_info *q)
-{
-        index_inc(&q->head, q->len);
-}
-
-static inline void queue_tail_inc(struct be_queue_info *q)
-{
-        index_inc(&q->tail, q->len);
-}
-
 static void be_intr_set(struct be_ctrl_info *ctrl, bool enable)
 {
         u8 __iomem *addr = ctrl->pcicfg + PCICFG_MEMBAR_CTRL_INT_CTRL_OFFSET;
@@ -127,7 +107,7 @@ static void be_eq_notify(struct be_ctrl_info *ctrl, u16 qid,
         iowrite32(val, ctrl->db + DB_EQ_OFFSET);
 }
 
-static void be_cq_notify(struct be_ctrl_info *ctrl, u16 qid,
+void be_cq_notify(struct be_ctrl_info *ctrl, u16 qid,
                 bool arm, u16 num_popped)
 {
         u32 val = 0;
@@ -234,28 +214,24 @@ static void netdev_stats_update(struct be_adapter *adapter)
         dev_stats->tx_window_errors = 0;
 }
 
-static void be_link_status_update(struct be_adapter *adapter)
+void be_link_status_update(void *ctxt, bool link_up)
 {
-        struct be_link_info *prev = &adapter->link;
-        struct be_link_info now = { 0 };
+        struct be_adapter *adapter = ctxt;
         struct net_device *netdev = adapter->netdev;
 
-        be_cmd_link_status_query(&adapter->ctrl, &now);
-
         /* If link came up or went down */
-        if (now.speed != prev->speed && (now.speed == PHY_LINK_SPEED_ZERO ||
-                        prev->speed == PHY_LINK_SPEED_ZERO)) {
-                if (now.speed == PHY_LINK_SPEED_ZERO) {
-                        netif_stop_queue(netdev);
-                        netif_carrier_off(netdev);
-                        printk(KERN_INFO "%s: Link down\n", netdev->name);
-                } else {
+        if (adapter->link_up != link_up) {
+                if (link_up) {
                         netif_start_queue(netdev);
                         netif_carrier_on(netdev);
                         printk(KERN_INFO "%s: Link up\n", netdev->name);
+                } else {
+                        netif_stop_queue(netdev);
+                        netif_carrier_off(netdev);
+                        printk(KERN_INFO "%s: Link down\n", netdev->name);
                 }
+                adapter->link_up = link_up;
         }
-        *prev = now;
 }
 
 /* Update the EQ delay n BE based on the RX frags consumed / sec */
@@ -569,47 +545,32 @@ static void be_vlan_rem_vid(struct net_device *netdev, u16 vid)
         be_vid_config(netdev);
 }
 
-static void be_set_multicast_filter(struct net_device *netdev)
+static void be_set_multicast_list(struct net_device *netdev)
 {
         struct be_adapter *adapter = netdev_priv(netdev);
-        struct dev_mc_list *mc_ptr;
-        u8 mac_addr[32][ETH_ALEN];
-        int i = 0;
+        struct be_ctrl_info *ctrl = &adapter->ctrl;
 
-        if (netdev->flags & IFF_ALLMULTI) {
-                /* set BE in Multicast promiscuous */
-                be_cmd_mcast_mac_set(&adapter->ctrl,
-                                        adapter->if_handle, NULL, 0, true);
-                return;
+        if (netdev->flags & IFF_PROMISC) {
+                be_cmd_promiscuous_config(ctrl, adapter->port_num, 1);
+                adapter->promiscuous = true;
+                goto done;
         }
 
-        for (mc_ptr = netdev->mc_list; mc_ptr; mc_ptr = mc_ptr->next) {
-                memcpy(&mac_addr[i][0], mc_ptr->dmi_addr, ETH_ALEN);
-                if (++i >= 32) {
-                        be_cmd_mcast_mac_set(&adapter->ctrl,
-                                adapter->if_handle, &mac_addr[0][0], i, false);
-                        i = 0;
-                }
-
+        /* BE was previously in promiscous mode; disable it */
+        if (adapter->promiscuous) {
+                adapter->promiscuous = false;
+                be_cmd_promiscuous_config(ctrl, adapter->port_num, 0);
         }
 
-        if (i) {
-                /* reset the promiscuous mode also. */
-                be_cmd_mcast_mac_set(&adapter->ctrl,
-                        adapter->if_handle, &mac_addr[0][0], i, false);
+        if (netdev->flags & IFF_ALLMULTI) {
+                be_cmd_multicast_set(ctrl, adapter->if_handle, NULL, 0);
+                goto done;
         }
-}
 
-static void be_set_multicast_list(struct net_device *netdev)
-{
-        struct be_adapter *adapter = netdev_priv(netdev);
-
-        if (netdev->flags & IFF_PROMISC) {
-                be_cmd_promiscuous_config(&adapter->ctrl, adapter->port_num, 1);
-        } else {
-                be_cmd_promiscuous_config(&adapter->ctrl, adapter->port_num, 0);
-                be_set_multicast_filter(netdev);
-        }
+        be_cmd_multicast_set(ctrl, adapter->if_handle, netdev->mc_list,
+                netdev->mc_count);
+done:
+        return;
 }
 
 static void be_rx_rate_update(struct be_adapter *adapter)
@@ -960,10 +921,8 @@ static void be_post_rx_frags(struct be_adapter *adapter)
         return;
 }
 
-static struct be_eth_tx_compl *
-be_tx_compl_get(struct be_adapter *adapter)
+static struct be_eth_tx_compl *be_tx_compl_get(struct be_queue_info *tx_cq)
 {
-        struct be_queue_info *tx_cq = &adapter->tx_obj.cq;
         struct be_eth_tx_compl *txcp = queue_tail_node(tx_cq);
 
         if (txcp->dw[offsetof(struct amap_eth_tx_compl, valid) / 32] == 0)
@@ -1051,6 +1010,59 @@ static void be_tx_q_clean(struct be_adapter *adapter)
         }
 }
 
+static void be_mcc_queues_destroy(struct be_adapter *adapter)
+{
+        struct be_queue_info *q;
+        struct be_ctrl_info *ctrl = &adapter->ctrl;
+
+        q = &ctrl->mcc_obj.q;
+        if (q->created)
+                be_cmd_q_destroy(ctrl, q, QTYPE_MCCQ);
+        be_queue_free(adapter, q);
+
+        q = &ctrl->mcc_obj.cq;
+        if (q->created)
+                be_cmd_q_destroy(ctrl, q, QTYPE_CQ);
+        be_queue_free(adapter, q);
+}
+
+/* Must be called only after TX qs are created as MCC shares TX EQ */
+static int be_mcc_queues_create(struct be_adapter *adapter)
+{
+        struct be_queue_info *q, *cq;
+        struct be_ctrl_info *ctrl = &adapter->ctrl;
+
+        /* Alloc MCC compl queue */
+        cq = &ctrl->mcc_obj.cq;
+        if (be_queue_alloc(adapter, cq, MCC_CQ_LEN,
+                        sizeof(struct be_mcc_cq_entry)))
+                goto err;
+
+        /* Ask BE to create MCC compl queue; share TX's eq */
+        if (be_cmd_cq_create(ctrl, cq, &adapter->tx_eq.q, false, true, 0))
+                goto mcc_cq_free;
+
+        /* Alloc MCC queue */
+        q = &ctrl->mcc_obj.q;
+        if (be_queue_alloc(adapter, q, MCC_Q_LEN, sizeof(struct be_mcc_wrb)))
+                goto mcc_cq_destroy;
+
+        /* Ask BE to create MCC queue */
+        if (be_cmd_mccq_create(ctrl, q, cq))
+                goto mcc_q_free;
+
+        return 0;
+
+mcc_q_free:
+        be_queue_free(adapter, q);
+mcc_cq_destroy:
+        be_cmd_q_destroy(ctrl, cq, QTYPE_CQ);
+mcc_cq_free:
+        be_queue_free(adapter, cq);
+err:
+        return -1;
+}
+
 static void be_tx_queues_destroy(struct be_adapter *adapter)
 {
         struct be_queue_info *q;
@@ -1263,7 +1275,7 @@ static irqreturn_t be_msix_rx(int irq, void *dev)
         return IRQ_HANDLED;
 }
 
-static irqreturn_t be_msix_tx(int irq, void *dev)
+static irqreturn_t be_msix_tx_mcc(int irq, void *dev)
 {
         struct be_adapter *adapter = dev;
 
@@ -1324,40 +1336,51 @@ int be_poll_rx(struct napi_struct *napi, int budget)
         return work_done;
 }
 
-/* For TX we don't honour budget; consume everything */
-int be_poll_tx(struct napi_struct *napi, int budget)
+void be_process_tx(struct be_adapter *adapter)
 {
-        struct be_eq_obj *tx_eq = container_of(napi, struct be_eq_obj, napi);
-        struct be_adapter *adapter =
-                container_of(tx_eq, struct be_adapter, tx_eq);
-        struct be_tx_obj *tx_obj = &adapter->tx_obj;
-        struct be_queue_info *tx_cq = &tx_obj->cq;
-        struct be_queue_info *txq = &tx_obj->q;
+        struct be_queue_info *txq = &adapter->tx_obj.q;
+        struct be_queue_info *tx_cq = &adapter->tx_obj.cq;
         struct be_eth_tx_compl *txcp;
         u32 num_cmpl = 0;
         u16 end_idx;
 
-        while ((txcp = be_tx_compl_get(adapter))) {
+        while ((txcp = be_tx_compl_get(tx_cq))) {
                 end_idx = AMAP_GET_BITS(struct amap_eth_tx_compl,
                                         wrb_index, txcp);
                 be_tx_compl_process(adapter, end_idx);
                 num_cmpl++;
         }
 
-        /* As Tx wrbs have been freed up, wake up netdev queue if
-         * it was stopped due to lack of tx wrbs.
-         */
-        if (netif_queue_stopped(adapter->netdev) &&
+        if (num_cmpl) {
+                be_cq_notify(&adapter->ctrl, tx_cq->id, true, num_cmpl);
+
+                /* As Tx wrbs have been freed up, wake up netdev queue if
+                 * it was stopped due to lack of tx wrbs.
+                 */
+                if (netif_queue_stopped(adapter->netdev) &&
                         atomic_read(&txq->used) < txq->len / 2) {
-                netif_wake_queue(adapter->netdev);
+                        netif_wake_queue(adapter->netdev);
+                }
+
+                drvr_stats(adapter)->be_tx_events++;
+                drvr_stats(adapter)->be_tx_compl += num_cmpl;
         }
+}
+
+/* As TX and MCC share the same EQ check for both TX and MCC completions.
+ * For TX/MCC we don't honour budget; consume everything
+ */
+static int be_poll_tx_mcc(struct napi_struct *napi, int budget)
+{
+        struct be_eq_obj *tx_eq = container_of(napi, struct be_eq_obj, napi);
+        struct be_adapter *adapter =
+                container_of(tx_eq, struct be_adapter, tx_eq);
 
         napi_complete(napi);
 
-        be_cq_notify(&adapter->ctrl, tx_cq->id, true, num_cmpl);
+        be_process_tx(adapter);
 
-        drvr_stats(adapter)->be_tx_events++;
-        drvr_stats(adapter)->be_tx_compl += num_cmpl;
+        be_process_mcc(&adapter->ctrl);
 
         return 1;
 }
@@ -1368,9 +1391,6 @@ static void be_worker(struct work_struct *work)
                 container_of(work, struct be_adapter, work.work);
         int status;
 
-        /* Check link */
-        be_link_status_update(adapter);
-
         /* Get Stats */
         status = be_cmd_get_stats(&adapter->ctrl, &adapter->stats.cmd);
         if (!status)
@@ -1419,7 +1439,7 @@ static int be_msix_register(struct be_adapter *adapter)
 
         sprintf(tx_eq->desc, "%s-tx", netdev->name);
         vec = be_msix_vec_get(adapter, tx_eq->q.id);
-        status = request_irq(vec, be_msix_tx, 0, tx_eq->desc, adapter);
+        status = request_irq(vec, be_msix_tx_mcc, 0, tx_eq->desc, adapter);
         if (status)
                 goto err;
 
@@ -1495,6 +1515,39 @@ static int be_open(struct net_device *netdev)
         struct be_ctrl_info *ctrl = &adapter->ctrl;
         struct be_eq_obj *rx_eq = &adapter->rx_eq;
         struct be_eq_obj *tx_eq = &adapter->tx_eq;
+        bool link_up;
+        int status;
+
+        /* First time posting */
+        be_post_rx_frags(adapter);
+
+        napi_enable(&rx_eq->napi);
+        napi_enable(&tx_eq->napi);
+
+        be_irq_register(adapter);
+
+        be_intr_set(ctrl, true);
+
+        /* The evt queues are created in unarmed state; arm them */
+        be_eq_notify(ctrl, rx_eq->q.id, true, false, 0);
+        be_eq_notify(ctrl, tx_eq->q.id, true, false, 0);
+
+        /* Rx compl queue may be in unarmed state; rearm it */
+        be_cq_notify(ctrl, adapter->rx_obj.cq.id, true, 0);
+
+        status = be_cmd_link_status_query(ctrl, &link_up);
+        if (status)
+                return status;
+        be_link_status_update(adapter, link_up);
+
+        schedule_delayed_work(&adapter->work, msecs_to_jiffies(100));
+        return 0;
+}
+
+static int be_setup(struct be_adapter *adapter)
+{
+        struct be_ctrl_info *ctrl = &adapter->ctrl;
+        struct net_device *netdev = adapter->netdev;
         u32 if_flags;
         int status;
 
@@ -1521,29 +1574,14 @@ static int be_open(struct net_device *netdev)
         if (status != 0)
                 goto tx_qs_destroy;
 
-        /* First time posting */
-        be_post_rx_frags(adapter);
-
-        napi_enable(&rx_eq->napi);
-        napi_enable(&tx_eq->napi);
-
-        be_irq_register(adapter);
-
-        be_intr_set(ctrl, true);
-
-        /* The evt queues are created in the unarmed state; arm them */
-        be_eq_notify(ctrl, rx_eq->q.id, true, false, 0);
-        be_eq_notify(ctrl, tx_eq->q.id, true, false, 0);
-
-        /* The compl queues are created in the unarmed state; arm them */
-        be_cq_notify(ctrl, adapter->rx_obj.cq.id, true, 0);
-        be_cq_notify(ctrl, adapter->tx_obj.cq.id, true, 0);
-
-        be_link_status_update(adapter);
+        status = be_mcc_queues_create(adapter);
+        if (status != 0)
+                goto rx_qs_destroy;
 
-        schedule_delayed_work(&adapter->work, msecs_to_jiffies(100));
         return 0;
 
+rx_qs_destroy:
+        be_rx_queues_destroy(adapter);
 tx_qs_destroy:
         be_tx_queues_destroy(adapter);
 if_destroy:
@@ -1552,6 +1590,19 @@ do_none:
         return status;
 }
 
+static int be_clear(struct be_adapter *adapter)
+{
+        struct be_ctrl_info *ctrl = &adapter->ctrl;
+
+        be_rx_queues_destroy(adapter);
+        be_tx_queues_destroy(adapter);
+
+        be_cmd_if_destroy(ctrl, adapter->if_handle);
+
+        be_mcc_queues_destroy(adapter);
+        return 0;
+}
+
 static int be_close(struct net_device *netdev)
 {
         struct be_adapter *adapter = netdev_priv(netdev);
@@ -1564,7 +1615,7 @@ static int be_close(struct net_device *netdev)
 
         netif_stop_queue(netdev);
         netif_carrier_off(netdev);
-        adapter->link.speed = PHY_LINK_SPEED_ZERO;
+        adapter->link_up = false;
 
         be_intr_set(ctrl, false);
 
@@ -1581,10 +1632,6 @@ static int be_close(struct net_device *netdev)
         napi_disable(&rx_eq->napi);
         napi_disable(&tx_eq->napi);
 
-        be_rx_queues_destroy(adapter);
-        be_tx_queues_destroy(adapter);
-
-        be_cmd_if_destroy(ctrl, adapter->if_handle);
         return 0;
 }
 
@@ -1673,7 +1720,7 @@ static void be_netdev_init(struct net_device *netdev)
 
         netif_napi_add(netdev, &adapter->rx_eq.napi, be_poll_rx,
                 BE_NAPI_WEIGHT);
-        netif_napi_add(netdev, &adapter->tx_eq.napi, be_poll_tx,
+        netif_napi_add(netdev, &adapter->tx_eq.napi, be_poll_tx_mcc,
                 BE_NAPI_WEIGHT);
 
         netif_carrier_off(netdev);
@@ -1755,7 +1802,12 @@ static int be_ctrl_init(struct be_adapter *adapter)
         mbox_mem_align->va = PTR_ALIGN(mbox_mem_alloc->va, 16);
         mbox_mem_align->dma = PTR_ALIGN(mbox_mem_alloc->dma, 16);
         memset(mbox_mem_align->va, 0, sizeof(struct be_mcc_mailbox));
-        spin_lock_init(&ctrl->cmd_lock);
+        spin_lock_init(&ctrl->mbox_lock);
+        spin_lock_init(&ctrl->mcc_lock);
+        spin_lock_init(&ctrl->mcc_cq_lock);
+
+        ctrl->async_cb = be_link_status_update;
+        ctrl->adapter_ctxt = adapter;
 
         val = ioread32(ctrl->pcicfg + PCICFG_MEMBAR_CTRL_INT_CTRL_OFFSET);
         ctrl->pci_func = (val >> MEMBAR_CTRL_INT_CTRL_PFUNC_SHIFT) &
@@ -1793,6 +1845,8 @@ static void __devexit be_remove(struct pci_dev *pdev)
 
         unregister_netdev(adapter->netdev);
 
+        be_clear(adapter);
+
         be_stats_cleanup(adapter);
 
         be_ctrl_cleanup(adapter);
@@ -1890,13 +1944,18 @@ static int __devinit be_probe(struct pci_dev *pdev,
         be_netdev_init(netdev);
         SET_NETDEV_DEV(netdev, &adapter->pdev->dev);
 
+        status = be_setup(adapter);
+        if (status)
+                goto stats_clean;
         status = register_netdev(netdev);
         if (status != 0)
-                goto stats_clean;
+                goto unsetup;
 
         dev_info(&pdev->dev, "%s port %d\n", nic_name(pdev), adapter->port_num);
         return 0;
 
+unsetup:
+        be_clear(adapter);
 stats_clean:
         be_stats_cleanup(adapter);
 ctrl_clean:
@@ -1921,6 +1980,7 @@ static int be_suspend(struct pci_dev *pdev, pm_message_t state)
         if (netif_running(netdev)) {
                 rtnl_lock();
                 be_close(netdev);
+                be_clear(adapter);
                 rtnl_unlock();
         }
 
@@ -1947,6 +2007,7 @@ static int be_resume(struct pci_dev *pdev)
 
         if (netif_running(netdev)) {
                 rtnl_lock();
+                be_setup(adapter);
                 be_open(netdev);
                 rtnl_unlock();
         }
diff --git a/drivers/net/e1000e/netdev.c b/drivers/net/e1000e/netdev.c
index 677f60490f67..679885a122b4 100644
--- a/drivers/net/e1000e/netdev.c
+++ b/drivers/net/e1000e/netdev.c
@@ -1997,7 +1997,7 @@ static int e1000_clean(struct napi_struct *napi, int budget)
         struct e1000_adapter *adapter = container_of(napi, struct e1000_adapter, napi);
         struct e1000_hw *hw = &adapter->hw;
         struct net_device *poll_dev = adapter->netdev;
-        int tx_cleaned = 0, work_done = 0;
+        int tx_cleaned = 1, work_done = 0;
 
         adapter = netdev_priv(poll_dev);
 
diff --git a/drivers/net/mlx4/en_netdev.c b/drivers/net/mlx4/en_netdev.c
index e02bafdd3682..93f4abd990a9 100644
--- a/drivers/net/mlx4/en_netdev.c
+++ b/drivers/net/mlx4/en_netdev.c
@@ -668,7 +668,7 @@ int mlx4_en_start_port(struct net_device *dev)
         queue_work(mdev->workqueue, &priv->mcast_task);
 
         priv->port_up = true;
-        netif_start_queue(dev);
+        netif_tx_start_all_queues(dev);
         return 0;
 
 mac_err:
@@ -700,14 +700,14 @@ void mlx4_en_stop_port(struct net_device *dev)
                 en_dbg(DRV, priv, "stop port called while port already down\n");
                 return;
         }
-        netif_stop_queue(dev);
 
         /* Synchronize with tx routine */
         netif_tx_lock_bh(dev);
-        priv->port_up = false;
+        netif_tx_stop_all_queues(dev);
         netif_tx_unlock_bh(dev);
 
         /* close port*/
+        priv->port_up = false;
         mlx4_CLOSE_PORT(mdev->dev, priv->port);
 
         /* Unregister Mac address for the port */
@@ -881,7 +881,6 @@ void mlx4_en_destroy_netdev(struct net_device *dev)
                 mlx4_free_hwq_res(mdev->dev, &priv->res, MLX4_EN_PAGE_SIZE);
 
         cancel_delayed_work(&priv->stats_task);
-        cancel_delayed_work(&priv->refill_task);
         /* flush any pending task for this netdev */
         flush_workqueue(mdev->workqueue);
 
@@ -986,7 +985,6 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
         spin_lock_init(&priv->stats_lock);
         INIT_WORK(&priv->mcast_task, mlx4_en_do_set_multicast);
         INIT_WORK(&priv->mac_task, mlx4_en_do_set_mac);
-        INIT_DELAYED_WORK(&priv->refill_task, mlx4_en_rx_refill);
         INIT_WORK(&priv->watchdog_task, mlx4_en_restart);
         INIT_WORK(&priv->linkstate_task, mlx4_en_linkstate);
         INIT_DELAYED_WORK(&priv->stats_task, mlx4_en_do_get_stats);
diff --git a/drivers/net/mlx4/en_rx.c b/drivers/net/mlx4/en_rx.c
index 5a14899c1e25..91bdfdfd431f 100644
--- a/drivers/net/mlx4/en_rx.c
+++ b/drivers/net/mlx4/en_rx.c
@@ -269,31 +269,6 @@ reduce_rings:
         return 0;
 }
 
-static int mlx4_en_fill_rx_buf(struct net_device *dev,
-                               struct mlx4_en_rx_ring *ring)
-{
-        struct mlx4_en_priv *priv = netdev_priv(dev);
-        int num = 0;
-        int err;
-
-        while ((u32) (ring->prod - ring->cons) < ring->actual_size) {
-                err = mlx4_en_prepare_rx_desc(priv, ring, ring->prod &
-                                              ring->size_mask);
-                if (err) {
-                        if (netif_msg_rx_err(priv))
-                                en_warn(priv, "Failed preparing rx descriptor\n");
-                        priv->port_stats.rx_alloc_failed++;
-                        break;
-                }
-                ++num;
-                ++ring->prod;
-        }
-        if ((u32) (ring->prod - ring->cons) == ring->actual_size)
-                ring->full = 1;
-
-        return num;
-}
-
 static void mlx4_en_free_rx_buf(struct mlx4_en_priv *priv,
                                 struct mlx4_en_rx_ring *ring)
 {
@@ -312,42 +287,6 @@ static void mlx4_en_free_rx_buf(struct mlx4_en_priv *priv,
         }
 }
 
-
-void mlx4_en_rx_refill(struct work_struct *work)
-{
-        struct delayed_work *delay = to_delayed_work(work);
-        struct mlx4_en_priv *priv = container_of(delay, struct mlx4_en_priv,
-                                                 refill_task);
-        struct mlx4_en_dev *mdev = priv->mdev;
-        struct net_device *dev = priv->dev;
-        struct mlx4_en_rx_ring *ring;
-        int need_refill = 0;
-        int i;
-
-        mutex_lock(&mdev->state_lock);
-        if (!mdev->device_up || !priv->port_up)
-                goto out;
-
-        /* We only get here if there are no receive buffers, so we can't race
-         * with Rx interrupts while filling buffers */
-        for (i = 0; i < priv->rx_ring_num; i++) {
-                ring = &priv->rx_ring[i];
-                if (ring->need_refill) {
-                        if (mlx4_en_fill_rx_buf(dev, ring)) {
-                                ring->need_refill = 0;
-                                mlx4_en_update_rx_prod_db(ring);
-                        } else
-                                need_refill = 1;
-                }
-        }
-        if (need_refill)
-                queue_delayed_work(mdev->workqueue, &priv->refill_task, HZ);
-
-out:
-        mutex_unlock(&mdev->state_lock);
-}
-
-
 int mlx4_en_create_rx_ring(struct mlx4_en_priv *priv,
                            struct mlx4_en_rx_ring *ring, u32 size, u16 stride)
 {
@@ -457,9 +396,6 @@ int mlx4_en_activate_rx_rings(struct mlx4_en_priv *priv)
                         ring_ind--;
                         goto err_allocator;
                 }
-
-                /* Fill Rx buffers */
-                ring->full = 0;
         }
         err = mlx4_en_fill_rx_buffers(priv);
         if (err)
@@ -647,33 +583,6 @@ static struct sk_buff *mlx4_en_rx_skb(struct mlx4_en_priv *priv,
         return skb;
 }
 
-static void mlx4_en_copy_desc(struct mlx4_en_priv *priv,
-                              struct mlx4_en_rx_ring *ring,
-                              int from, int to, int num)
-{
-        struct skb_frag_struct *skb_frags_from;
-        struct skb_frag_struct *skb_frags_to;
-        struct mlx4_en_rx_desc *rx_desc_from;
-        struct mlx4_en_rx_desc *rx_desc_to;
-        int from_index, to_index;
-        int nr, i;
-
-        for (i = 0; i < num; i++) {
-                from_index = (from + i) & ring->size_mask;
-                to_index = (to + i) & ring->size_mask;
-                skb_frags_from = ring->rx_info + (from_index << priv->log_rx_info);
-                skb_frags_to = ring->rx_info + (to_index << priv->log_rx_info);
-                rx_desc_from = ring->buf + (from_index << ring->log_stride);
-                rx_desc_to = ring->buf + (to_index << ring->log_stride);
-
-                for (nr = 0; nr < priv->num_frags; nr++) {
-                        skb_frags_to[nr].page = skb_frags_from[nr].page;
-                        skb_frags_to[nr].page_offset = skb_frags_from[nr].page_offset;
-                        rx_desc_to->data[nr].addr = rx_desc_from->data[nr].addr;
-                }
-        }
-}
-
 
 int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int budget)
 {
@@ -821,11 +730,6 @@ out:
         wmb(); /* ensure HW sees CQ consumer before we post new buffers */
         ring->cons = cq->mcq.cons_index;
         ring->prod += polled; /* Polled descriptors were realocated in place */
-        if (unlikely(!ring->full)) {
-                mlx4_en_copy_desc(priv, ring, ring->cons - polled,
-                                  ring->prod - polled, polled);
-                mlx4_en_fill_rx_buf(dev, ring);
-        }
         mlx4_en_update_rx_prod_db(ring);
         return polled;
 }
diff --git a/drivers/net/mlx4/en_tx.c b/drivers/net/mlx4/en_tx.c
index 5dc7466ad035..08c43f2ae72b 100644
--- a/drivers/net/mlx4/en_tx.c
+++ b/drivers/net/mlx4/en_tx.c
@@ -515,16 +515,9 @@ static int get_real_size(struct sk_buff *skb, struct net_device *dev,
                         else {
                                 if (netif_msg_tx_err(priv))
                                         en_warn(priv, "Non-linear headers\n");
-                                dev_kfree_skb_any(skb);
                                 return 0;
                         }
                 }
-                if (unlikely(*lso_header_size > MAX_LSO_HDR_SIZE)) {
-                        if (netif_msg_tx_err(priv))
-                                en_warn(priv, "LSO header size too big\n");
-                        dev_kfree_skb_any(skb);
-                        return 0;
-                }
         } else {
                 *lso_header_size = 0;
                 if (!is_inline(skb, NULL))
@@ -616,13 +609,9 @@ int mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
         int lso_header_size;
         void *fragptr;
 
-        if (unlikely(!skb->len)) {
-                dev_kfree_skb_any(skb);
-                return NETDEV_TX_OK;
-        }
         real_size = get_real_size(skb, dev, &lso_header_size);
         if (unlikely(!real_size))
-                return NETDEV_TX_OK;
+                goto tx_drop;
 
         /* Allign descriptor to TXBB size */
         desc_size = ALIGN(real_size, TXBB_SIZE);
@@ -630,8 +619,7 @@ int mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
         if (unlikely(nr_txbb > MAX_DESC_TXBBS)) {
                 if (netif_msg_tx_err(priv))
                         en_warn(priv, "Oversized header or SG list\n");
-                dev_kfree_skb_any(skb);
-                return NETDEV_TX_OK;
+                goto tx_drop;
         }
 
         tx_ind = skb->queue_mapping;
@@ -653,14 +641,6 @@ int mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
                 return NETDEV_TX_BUSY;
         }
 
-        /* Now that we know what Tx ring to use */
-        if (unlikely(!priv->port_up)) {
-                if (netif_msg_tx_err(priv))
-                        en_warn(priv, "xmit: port down!\n");
-                dev_kfree_skb_any(skb);
-                return NETDEV_TX_OK;
-        }
-
         /* Track current inflight packets for performance analysis */
         AVG_PERF_COUNTER(priv->pstats.inflight_avg,
                          (u32) (ring->prod - ring->cons - 1));
@@ -785,5 +765,10 @@ int mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
         mlx4_en_xmit_poll(priv, tx_ind);
 
         return 0;
+
+tx_drop:
+        dev_kfree_skb_any(skb);
+        priv->stats.tx_dropped++;
+        return NETDEV_TX_OK;
 }
 
diff --git a/drivers/net/mlx4/mlx4_en.h b/drivers/net/mlx4/mlx4_en.h
index d43a9e4c2aea..c7c5e86804ff 100644
--- a/drivers/net/mlx4/mlx4_en.h
+++ b/drivers/net/mlx4/mlx4_en.h
@@ -99,7 +99,6 @@
 #define RSS_FACTOR              2
 #define TXBB_SIZE               64
 #define HEADROOM                (2048 / TXBB_SIZE + 1)
-#define MAX_LSO_HDR_SIZE        92
 #define STAMP_STRIDE            64
 #define STAMP_DWORDS            (STAMP_STRIDE / 4)
 #define STAMP_SHIFT             31
@@ -296,8 +295,6 @@ struct mlx4_en_rx_ring {
         u32 prod;
         u32 cons;
         u32 buf_size;
-        int need_refill;
-        int full;
         void *buf;
         void *rx_info;
         unsigned long bytes;
@@ -495,7 +492,6 @@ struct mlx4_en_priv {
         struct mlx4_en_cq rx_cq[MAX_RX_RINGS];
         struct work_struct mcast_task;
         struct work_struct mac_task;
-        struct delayed_work refill_task;
         struct work_struct watchdog_task;
         struct work_struct linkstate_task;
         struct delayed_work stats_task;
@@ -565,7 +561,6 @@ void mlx4_en_set_default_rss_map(struct mlx4_en_priv *priv,
 int mlx4_en_config_rss_steer(struct mlx4_en_priv *priv);
 void mlx4_en_release_rss_steer(struct mlx4_en_priv *priv);
 int mlx4_en_free_tx_buf(struct net_device *dev, struct mlx4_en_tx_ring *ring);
-void mlx4_en_rx_refill(struct work_struct *work);
 void mlx4_en_rx_irq(struct mlx4_cq *mcq);
 
 int mlx4_SET_MCAST_FLTR(struct mlx4_dev *dev, u8 port, u64 mac, u64 clear, u8 mode);
diff --git a/drivers/net/mv643xx_eth.c b/drivers/net/mv643xx_eth.c
index 745ae8b4a2e8..0f32db3e92ad 100644
--- a/drivers/net/mv643xx_eth.c
+++ b/drivers/net/mv643xx_eth.c
@@ -1750,12 +1750,12 @@ static void mv643xx_eth_program_unicast_filter(struct net_device *dev)
 
         uc_addr_set(mp, dev->dev_addr);
 
-        port_config = rdlp(mp, PORT_CONFIG);
+        port_config = rdlp(mp, PORT_CONFIG) & ~UNICAST_PROMISCUOUS_MODE;
+
         nibbles = uc_addr_filter_mask(dev);
         if (!nibbles) {
                 port_config |= UNICAST_PROMISCUOUS_MODE;
-                wrlp(mp, PORT_CONFIG, port_config);
-                return;
+                nibbles = 0xffff;
         }
 
         for (i = 0; i < 16; i += 4) {
@@ -1776,7 +1776,6 @@ static void mv643xx_eth_program_unicast_filter(struct net_device *dev)
                 wrl(mp, off, v);
         }
 
-        port_config &= ~UNICAST_PROMISCUOUS_MODE;
         wrlp(mp, PORT_CONFIG, port_config);
 }
 
diff --git a/drivers/net/ppp_async.c b/drivers/net/ppp_async.c
index 6de8399d6dd9..17c116bb332c 100644
--- a/drivers/net/ppp_async.c
+++ b/drivers/net/ppp_async.c
@@ -356,7 +356,6 @@ ppp_asynctty_receive(struct tty_struct *tty, const unsigned char *buf,
         if (!skb_queue_empty(&ap->rqueue))
                 tasklet_schedule(&ap->tsk);
         ap_put(ap);
-        tty_unthrottle(tty);
 }
 
 static void
diff --git a/drivers/net/ppp_synctty.c b/drivers/net/ppp_synctty.c
index d2fa2db13586..aa3d39f38e22 100644
--- a/drivers/net/ppp_synctty.c
+++ b/drivers/net/ppp_synctty.c
@@ -397,7 +397,6 @@ ppp_sync_receive(struct tty_struct *tty, const unsigned char *buf,
         if (!skb_queue_empty(&ap->rqueue))
                 tasklet_schedule(&ap->tsk);
         sp_put(ap);
-        tty_unthrottle(tty);
 }
 
 static void
diff --git a/drivers/net/qla3xxx.c b/drivers/net/qla3xxx.c
index 8a823ecc99a9..bbc6d4d3cc94 100644
--- a/drivers/net/qla3xxx.c
+++ b/drivers/net/qla3xxx.c
@@ -3837,7 +3837,9 @@ static void ql_reset_work(struct work_struct *work)
                                                       16) | ISP_CONTROL_RI));
                         }
 
+                        spin_unlock_irqrestore(&qdev->hw_lock, hw_flags);
                         ssleep(1);
+                        spin_lock_irqsave(&qdev->hw_lock, hw_flags);
                 } while (--max_wait_time);
                 spin_unlock_irqrestore(&qdev->hw_lock, hw_flags);
 
diff --git a/drivers/net/r8169.c b/drivers/net/r8169.c
index 4e22462684c9..4b53b58d75fc 100644
--- a/drivers/net/r8169.c
+++ b/drivers/net/r8169.c
@@ -51,9 +51,6 @@
 #define TX_BUFFS_AVAIL(tp) \
         (tp->dirty_tx + NUM_TX_DESC - tp->cur_tx - 1)
 
-/* Maximum events (Rx packets, etc.) to handle at each interrupt. */
-static const int max_interrupt_work = 20;
-
 /* Maximum number of multicast addresses to filter (vs. Rx-all-multicast).
    The RTL chips use a 64 element hash table based on the Ethernet CRC. */
 static const int multicast_filter_limit = 32;
diff --git a/drivers/net/s6gmac.c b/drivers/net/s6gmac.c
new file mode 100644
index 000000000000..5345e47b35ac
--- /dev/null
+++ b/drivers/net/s6gmac.c
@@ -0,0 +1,1073 @@
+/*
+ * Ethernet driver for S6105 on chip network device
+ * (c)2008 emlix GmbH http://www.emlix.com
+ * Authors:     Oskar Schirmer <os@emlix.com>
+ *              Daniel Gloeckner <dg@emlix.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/if.h>
+#include <linux/stddef.h>
+#include <linux/mii.h>
+#include <linux/phy.h>
+#include <linux/platform_device.h>
+#include <variant/hardware.h>
+#include <variant/dmac.h>
+
+#define DRV_NAME "s6gmac"
+#define DRV_PRMT DRV_NAME ": "
+
+
+/* register declarations */
+
+#define S6_GMAC_MACCONF1        0x000
+#define S6_GMAC_MACCONF1_TXENA          0
+#define S6_GMAC_MACCONF1_SYNCTX         1
+#define S6_GMAC_MACCONF1_RXENA          2
+#define S6_GMAC_MACCONF1_SYNCRX         3
+#define S6_GMAC_MACCONF1_TXFLOWCTRL     4
+#define S6_GMAC_MACCONF1_RXFLOWCTRL     5
+#define S6_GMAC_MACCONF1_LOOPBACK       8
+#define S6_GMAC_MACCONF1_RESTXFUNC      16
+#define S6_GMAC_MACCONF1_RESRXFUNC      17
+#define S6_GMAC_MACCONF1_RESTXMACCTRL   18
+#define S6_GMAC_MACCONF1_RESRXMACCTRL   19
+#define S6_GMAC_MACCONF1_SIMULRES       30
+#define S6_GMAC_MACCONF1_SOFTRES        31
+#define S6_GMAC_MACCONF2        0x004
+#define S6_GMAC_MACCONF2_FULL           0
+#define S6_GMAC_MACCONF2_CRCENA         1
+#define S6_GMAC_MACCONF2_PADCRCENA      2
+#define S6_GMAC_MACCONF2_LENGTHFCHK     4
+#define S6_GMAC_MACCONF2_HUGEFRAMENA    5
+#define S6_GMAC_MACCONF2_IFMODE         8
+#define S6_GMAC_MACCONF2_IFMODE_NIBBLE          1
+#define S6_GMAC_MACCONF2_IFMODE_BYTE            2
+#define S6_GMAC_MACCONF2_IFMODE_MASK            3
+#define S6_GMAC_MACCONF2_PREAMBLELEN    12
+#define S6_GMAC_MACCONF2_PREAMBLELEN_MASK       0x0F
+#define S6_GMAC_MACIPGIFG       0x008
+#define S6_GMAC_MACIPGIFG_B2BINTERPGAP  0
+#define S6_GMAC_MACIPGIFG_B2BINTERPGAP_MASK     0x7F
+#define S6_GMAC_MACIPGIFG_MINIFGENFORCE 8
+#define S6_GMAC_MACIPGIFG_B2BINTERPGAP2 16
+#define S6_GMAC_MACIPGIFG_B2BINTERPGAP1 24
+#define S6_GMAC_MACHALFDUPLEX   0x00C
+#define S6_GMAC_MACHALFDUPLEX_COLLISWIN 0
+#define S6_GMAC_MACHALFDUPLEX_COLLISWIN_MASK    0x3F
+#define S6_GMAC_MACHALFDUPLEX_RETXMAX   12
+#define S6_GMAC_MACHALFDUPLEX_RETXMAX_MASK      0x0F
+#define S6_GMAC_MACHALFDUPLEX_EXCESSDEF 16
+#define S6_GMAC_MACHALFDUPLEX_NOBACKOFF 17
+#define S6_GMAC_MACHALFDUPLEX_BPNOBCKOF 18
+#define S6_GMAC_MACHALFDUPLEX_ALTBEBENA 19
+#define S6_GMAC_MACHALFDUPLEX_ALTBEBTRN 20
+#define S6_GMAC_MACHALFDUPLEX_ALTBEBTR_MASK     0x0F
+#define S6_GMAC_MACMAXFRAMELEN  0x010
+#define S6_GMAC_MACMIICONF      0x020
+#define S6_GMAC_MACMIICONF_CSEL         0
+#define S6_GMAC_MACMIICONF_CSEL_DIV10           0
+#define S6_GMAC_MACMIICONF_CSEL_DIV12           1
+#define S6_GMAC_MACMIICONF_CSEL_DIV14           2
+#define S6_GMAC_MACMIICONF_CSEL_DIV18           3
+#define S6_GMAC_MACMIICONF_CSEL_DIV24           4
+#define S6_GMAC_MACMIICONF_CSEL_DIV34           5
+#define S6_GMAC_MACMIICONF_CSEL_DIV68           6
+#define S6_GMAC_MACMIICONF_CSEL_DIV168          7
+#define S6_GMAC_MACMIICONF_CSEL_MASK            7
+#define S6_GMAC_MACMIICONF_PREAMBLESUPR 4
+#define S6_GMAC_MACMIICONF_SCANAUTOINCR 5
+#define S6_GMAC_MACMIICMD       0x024
+#define S6_GMAC_MACMIICMD_READ          0
+#define S6_GMAC_MACMIICMD_SCAN          1
+#define S6_GMAC_MACMIIADDR      0x028
+#define S6_GMAC_MACMIIADDR_REG          0
+#define S6_GMAC_MACMIIADDR_REG_MASK             0x1F
+#define S6_GMAC_MACMIIADDR_PHY          8
+#define S6_GMAC_MACMIIADDR_PHY_MASK             0x1F
+#define S6_GMAC_MACMIICTRL      0x02C
+#define S6_GMAC_MACMIISTAT      0x030
+#define S6_GMAC_MACMIIINDI      0x034
+#define S6_GMAC_MACMIIINDI_BUSY         0
+#define S6_GMAC_MACMIIINDI_SCAN         1
+#define S6_GMAC_MACMIIINDI_INVAL        2
+#define S6_GMAC_MACINTERFSTAT   0x03C
+#define S6_GMAC_MACINTERFSTAT_LINKFAIL  3
+#define S6_GMAC_MACINTERFSTAT_EXCESSDEF 9
+#define S6_GMAC_MACSTATADDR1    0x040
+#define S6_GMAC_MACSTATADDR2    0x044
+
+#define S6_GMAC_FIFOCONF0       0x048
+#define S6_GMAC_FIFOCONF0_HSTRSTWT      0
+#define S6_GMAC_FIFOCONF0_HSTRSTSR      1
+#define S6_GMAC_FIFOCONF0_HSTRSTFR      2
+#define S6_GMAC_FIFOCONF0_HSTRSTST      3
+#define S6_GMAC_FIFOCONF0_HSTRSTFT      4
+#define S6_GMAC_FIFOCONF0_WTMENREQ      8
+#define S6_GMAC_FIFOCONF0_SRFENREQ      9
+#define S6_GMAC_FIFOCONF0_FRFENREQ      10
+#define S6_GMAC_FIFOCONF0_STFENREQ      11
+#define S6_GMAC_FIFOCONF0_FTFENREQ      12
+#define S6_GMAC_FIFOCONF0_WTMENRPLY     16
+#define S6_GMAC_FIFOCONF0_SRFENRPLY     17
+#define S6_GMAC_FIFOCONF0_FRFENRPLY     18
+#define S6_GMAC_FIFOCONF0_STFENRPLY     19
+#define S6_GMAC_FIFOCONF0_FTFENRPLY     20
+#define S6_GMAC_FIFOCONF1       0x04C
+#define S6_GMAC_FIFOCONF2       0x050
+#define S6_GMAC_FIFOCONF2_CFGLWM        0
+#define S6_GMAC_FIFOCONF2_CFGHWM        16
+#define S6_GMAC_FIFOCONF3       0x054
+#define S6_GMAC_FIFOCONF3_CFGFTTH       0
+#define S6_GMAC_FIFOCONF3_CFGHWMFT      16
+#define S6_GMAC_FIFOCONF4       0x058
+#define S6_GMAC_FIFOCONF_RSV_PREVDROP   0
+#define S6_GMAC_FIFOCONF_RSV_RUNT       1
+#define S6_GMAC_FIFOCONF_RSV_FALSECAR   2
+#define S6_GMAC_FIFOCONF_RSV_CODEERR    3
+#define S6_GMAC_FIFOCONF_RSV_CRCERR     4
+#define S6_GMAC_FIFOCONF_RSV_LENGTHERR  5
+#define S6_GMAC_FIFOCONF_RSV_LENRANGE   6
+#define S6_GMAC_FIFOCONF_RSV_OK         7
+#define S6_GMAC_FIFOCONF_RSV_MULTICAST  8
+#define S6_GMAC_FIFOCONF_RSV_BROADCAST  9
+#define S6_GMAC_FIFOCONF_RSV_DRIBBLE    10
+#define S6_GMAC_FIFOCONF_RSV_CTRLFRAME  11
+#define S6_GMAC_FIFOCONF_RSV_PAUSECTRL  12
+#define S6_GMAC_FIFOCONF_RSV_UNOPCODE   13
+#define S6_GMAC_FIFOCONF_RSV_VLANTAG    14
+#define S6_GMAC_FIFOCONF_RSV_LONGEVENT  15
+#define S6_GMAC_FIFOCONF_RSV_TRUNCATED  16
+#define S6_GMAC_FIFOCONF_RSV_MASK               0x3FFFF
+#define S6_GMAC_FIFOCONF5       0x05C
+#define S6_GMAC_FIFOCONF5_DROPLT64      18
+#define S6_GMAC_FIFOCONF5_CFGBYTM       19
+#define S6_GMAC_FIFOCONF5_RXDROPSIZE    20
+#define S6_GMAC_FIFOCONF5_RXDROPSIZE_MASK       0xF
+
+#define S6_GMAC_STAT_REGS       0x080
+#define S6_GMAC_STAT_SIZE_MIN           12
+#define S6_GMAC_STATTR64        0x080
+#define S6_GMAC_STATTR64_SIZE           18
+#define S6_GMAC_STATTR127       0x084
+#define S6_GMAC_STATTR127_SIZE          18
+#define S6_GMAC_STATTR255       0x088
+#define S6_GMAC_STATTR255_SIZE          18
+#define S6_GMAC_STATTR511       0x08C
+#define S6_GMAC_STATTR511_SIZE          18
+#define S6_GMAC_STATTR1K        0x090
+#define S6_GMAC_STATTR1K_SIZE           18
+#define S6_GMAC_STATTRMAX       0x094
+#define S6_GMAC_STATTRMAX_SIZE          18
+#define S6_GMAC_STATTRMGV       0x098
+#define S6_GMAC_STATTRMGV_SIZE          18
+#define S6_GMAC_STATRBYT        0x09C
+#define S6_GMAC_STATRBYT_SIZE           24
+#define S6_GMAC_STATRPKT        0x0A0
+#define S6_GMAC_STATRPKT_SIZE           18
+#define S6_GMAC_STATRFCS        0x0A4
+#define S6_GMAC_STATRFCS_SIZE           12
+#define S6_GMAC_STATRMCA        0x0A8
+#define S6_GMAC_STATRMCA_SIZE           18
+#define S6_GMAC_STATRBCA        0x0AC
+#define S6_GMAC_STATRBCA_SIZE           22
+#define S6_GMAC_STATRXCF        0x0B0
+#define S6_GMAC_STATRXCF_SIZE           18
+#define S6_GMAC_STATRXPF        0x0B4
+#define S6_GMAC_STATRXPF_SIZE           12
+#define S6_GMAC_STATRXUO        0x0B8
+#define S6_GMAC_STATRXUO_SIZE           12
+#define S6_GMAC_STATRALN        0x0BC
+#define S6_GMAC_STATRALN_SIZE           12
+#define S6_GMAC_STATRFLR        0x0C0
+#define S6_GMAC_STATRFLR_SIZE           16
+#define S6_GMAC_STATRCDE        0x0C4
+#define S6_GMAC_STATRCDE_SIZE           12
+#define S6_GMAC_STATRCSE        0x0C8
+#define S6_GMAC_STATRCSE_SIZE           12
+#define S6_GMAC_STATRUND        0x0CC
+#define S6_GMAC_STATRUND_SIZE           12
+#define S6_GMAC_STATROVR        0x0D0
+#define S6_GMAC_STATROVR_SIZE           12
+#define S6_GMAC_STATRFRG        0x0D4
+#define S6_GMAC_STATRFRG_SIZE           12
+#define S6_GMAC_STATRJBR        0x0D8
+#define S6_GMAC_STATRJBR_SIZE           12
+#define S6_GMAC_STATRDRP        0x0DC
+#define S6_GMAC_STATRDRP_SIZE           12
+#define S6_GMAC_STATTBYT        0x0E0
+#define S6_GMAC_STATTBYT_SIZE           24
+#define S6_GMAC_STATTPKT        0x0E4
+#define S6_GMAC_STATTPKT_SIZE           18
+#define S6_GMAC_STATTMCA        0x0E8
+#define S6_GMAC_STATTMCA_SIZE           18
+#define S6_GMAC_STATTBCA        0x0EC
+#define S6_GMAC_STATTBCA_SIZE           18
+#define S6_GMAC_STATTXPF        0x0F0
+#define S6_GMAC_STATTXPF_SIZE           12
+#define S6_GMAC_STATTDFR        0x0F4
+#define S6_GMAC_STATTDFR_SIZE           12
+#define S6_GMAC_STATTEDF        0x0F8
+#define S6_GMAC_STATTEDF_SIZE           12
+#define S6_GMAC_STATTSCL        0x0FC
+#define S6_GMAC_STATTSCL_SIZE           12
+#define S6_GMAC_STATTMCL        0x100
+#define S6_GMAC_STATTMCL_SIZE           12
+#define S6_GMAC_STATTLCL        0x104
+#define S6_GMAC_STATTLCL_SIZE           12
+#define S6_GMAC_STATTXCL        0x108
+#define S6_GMAC_STATTXCL_SIZE           12
+#define S6_GMAC_STATTNCL        0x10C
+#define S6_GMAC_STATTNCL_SIZE           13
+#define S6_GMAC_STATTPFH        0x110
+#define S6_GMAC_STATTPFH_SIZE           12
+#define S6_GMAC_STATTDRP        0x114
+#define S6_GMAC_STATTDRP_SIZE           12
+#define S6_GMAC_STATTJBR        0x118
+#define S6_GMAC_STATTJBR_SIZE           12
+#define S6_GMAC_STATTFCS        0x11C
+#define S6_GMAC_STATTFCS_SIZE           12
+#define S6_GMAC_STATTXCF        0x120
+#define S6_GMAC_STATTXCF_SIZE           12
+#define S6_GMAC_STATTOVR        0x124
+#define S6_GMAC_STATTOVR_SIZE           12
+#define S6_GMAC_STATTUND        0x128
+#define S6_GMAC_STATTUND_SIZE           12
+#define S6_GMAC_STATTFRG        0x12C
+#define S6_GMAC_STATTFRG_SIZE           12
+#define S6_GMAC_STATCARRY(n)    (0x130 + 4*(n))
+#define S6_GMAC_STATCARRYMSK(n) (0x138 + 4*(n))
+#define S6_GMAC_STATCARRY1_RDRP         0
+#define S6_GMAC_STATCARRY1_RJBR         1
+#define S6_GMAC_STATCARRY1_RFRG         2
+#define S6_GMAC_STATCARRY1_ROVR         3
+#define S6_GMAC_STATCARRY1_RUND         4
+#define S6_GMAC_STATCARRY1_RCSE         5
+#define S6_GMAC_STATCARRY1_RCDE         6
+#define S6_GMAC_STATCARRY1_RFLR         7
+#define S6_GMAC_STATCARRY1_RALN         8
+#define S6_GMAC_STATCARRY1_RXUO         9
+#define S6_GMAC_STATCARRY1_RXPF         10
+#define S6_GMAC_STATCARRY1_RXCF         11
+#define S6_GMAC_STATCARRY1_RBCA         12
+#define S6_GMAC_STATCARRY1_RMCA         13
+#define S6_GMAC_STATCARRY1_RFCS         14
+#define S6_GMAC_STATCARRY1_RPKT         15
+#define S6_GMAC_STATCARRY1_RBYT         16
+#define S6_GMAC_STATCARRY1_TRMGV        25
+#define S6_GMAC_STATCARRY1_TRMAX        26
+#define S6_GMAC_STATCARRY1_TR1K         27
+#define S6_GMAC_STATCARRY1_TR511        28
+#define S6_GMAC_STATCARRY1_TR255        29
+#define S6_GMAC_STATCARRY1_TR127        30
+#define S6_GMAC_STATCARRY1_TR64         31
+#define S6_GMAC_STATCARRY2_TDRP         0
+#define S6_GMAC_STATCARRY2_TPFH         1
+#define S6_GMAC_STATCARRY2_TNCL         2
+#define S6_GMAC_STATCARRY2_TXCL         3
+#define S6_GMAC_STATCARRY2_TLCL         4
+#define S6_GMAC_STATCARRY2_TMCL         5
+#define S6_GMAC_STATCARRY2_TSCL         6
+#define S6_GMAC_STATCARRY2_TEDF         7
+#define S6_GMAC_STATCARRY2_TDFR         8
+#define S6_GMAC_STATCARRY2_TXPF         9
+#define S6_GMAC_STATCARRY2_TBCA         10
+#define S6_GMAC_STATCARRY2_TMCA         11
+#define S6_GMAC_STATCARRY2_TPKT         12
+#define S6_GMAC_STATCARRY2_TBYT         13
+#define S6_GMAC_STATCARRY2_TFRG         14
+#define S6_GMAC_STATCARRY2_TUND         15
+#define S6_GMAC_STATCARRY2_TOVR         16
+#define S6_GMAC_STATCARRY2_TXCF         17
+#define S6_GMAC_STATCARRY2_TFCS         18
+#define S6_GMAC_STATCARRY2_TJBR         19
+
+#define S6_GMAC_HOST_PBLKCTRL   0x140
+#define S6_GMAC_HOST_PBLKCTRL_TXENA     0
+#define S6_GMAC_HOST_PBLKCTRL_RXENA     1
+#define S6_GMAC_HOST_PBLKCTRL_TXSRES    2
+#define S6_GMAC_HOST_PBLKCTRL_RXSRES    3
+#define S6_GMAC_HOST_PBLKCTRL_TXBSIZ    8
+#define S6_GMAC_HOST_PBLKCTRL_RXBSIZ    12
+#define S6_GMAC_HOST_PBLKCTRL_SIZ_16            4
+#define S6_GMAC_HOST_PBLKCTRL_SIZ_32            5
+#define S6_GMAC_HOST_PBLKCTRL_SIZ_64            6
+#define S6_GMAC_HOST_PBLKCTRL_SIZ_128           7
+#define S6_GMAC_HOST_PBLKCTRL_SIZ_MASK          0xF
+#define S6_GMAC_HOST_PBLKCTRL_STATENA   16
+#define S6_GMAC_HOST_PBLKCTRL_STATAUTOZ 17
+#define S6_GMAC_HOST_PBLKCTRL_STATCLEAR 18
+#define S6_GMAC_HOST_PBLKCTRL_RGMII     19
+#define S6_GMAC_HOST_INTMASK    0x144
+#define S6_GMAC_HOST_INTSTAT    0x148
+#define S6_GMAC_HOST_INT_TXBURSTOVER    3
+#define S6_GMAC_HOST_INT_TXPREWOVER     4
+#define S6_GMAC_HOST_INT_RXBURSTUNDER   5
+#define S6_GMAC_HOST_INT_RXPOSTRFULL    6
+#define S6_GMAC_HOST_INT_RXPOSTRUNDER   7
+#define S6_GMAC_HOST_RXFIFOHWM  0x14C
+#define S6_GMAC_HOST_CTRLFRAMXP 0x150
+#define S6_GMAC_HOST_DSTADDRLO(n) (0x160 + 8*(n))
+#define S6_GMAC_HOST_DSTADDRHI(n) (0x164 + 8*(n))
+#define S6_GMAC_HOST_DSTMASKLO(n) (0x180 + 8*(n))
+#define S6_GMAC_HOST_DSTMASKHI(n) (0x184 + 8*(n))
+
+#define S6_GMAC_BURST_PREWR     0x1B0
+#define S6_GMAC_BURST_PREWR_LEN         0
+#define S6_GMAC_BURST_PREWR_LEN_MASK            ((1 << 20) - 1)
+#define S6_GMAC_BURST_PREWR_CFE         20
+#define S6_GMAC_BURST_PREWR_PPE         21
+#define S6_GMAC_BURST_PREWR_FCS         22
+#define S6_GMAC_BURST_PREWR_PAD         23
+#define S6_GMAC_BURST_POSTRD    0x1D0
+#define S6_GMAC_BURST_POSTRD_LEN        0
+#define S6_GMAC_BURST_POSTRD_LEN_MASK           ((1 << 20) - 1)
+#define S6_GMAC_BURST_POSTRD_DROP       20
+
+
+/* data handling */
+
+#define S6_NUM_TX_SKB   8       /* must be larger than TX fifo size */
+#define S6_NUM_RX_SKB   16
+#define S6_MAX_FRLEN    1536
+
+struct s6gmac {
+        u32 reg;
+        u32 tx_dma;
+        u32 rx_dma;
+        u32 io;
+        u8 tx_chan;
+        u8 rx_chan;
+        spinlock_t lock;
+        u8 tx_skb_i, tx_skb_o;
+        u8 rx_skb_i, rx_skb_o;
+        struct sk_buff *tx_skb[S6_NUM_TX_SKB];
+        struct sk_buff *rx_skb[S6_NUM_RX_SKB];
+        unsigned long carry[sizeof(struct net_device_stats) / sizeof(long)];
+        unsigned long stats[sizeof(struct net_device_stats) / sizeof(long)];
+        struct phy_device *phydev;
+        struct {
+                struct mii_bus *bus;
+                int irq[PHY_MAX_ADDR];
+        } mii;
+        struct {
+                unsigned int mbit;
+                u8 giga;
+                u8 isup;
+                u8 full;
+        } link;
+};
+
+static void s6gmac_rx_fillfifo(struct s6gmac *pd)
+{
+        struct sk_buff *skb;
+        while ((((u8)(pd->rx_skb_i - pd->rx_skb_o)) < S6_NUM_RX_SKB)
+                        && (!s6dmac_fifo_full(pd->rx_dma, pd->rx_chan))
+                        && (skb = dev_alloc_skb(S6_MAX_FRLEN + 2))) {
+                pd->rx_skb[(pd->rx_skb_i++) % S6_NUM_RX_SKB] = skb;
+                s6dmac_put_fifo_cache(pd->rx_dma, pd->rx_chan,
+                        pd->io, (u32)skb->data, S6_MAX_FRLEN);
+        }
+}
+
+static void s6gmac_rx_interrupt(struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        u32 pfx;
+        struct sk_buff *skb;
+        while (((u8)(pd->rx_skb_i - pd->rx_skb_o)) >
+                        s6dmac_pending_count(pd->rx_dma, pd->rx_chan)) {
+                skb = pd->rx_skb[(pd->rx_skb_o++) % S6_NUM_RX_SKB];
+                pfx = readl(pd->reg + S6_GMAC_BURST_POSTRD);
+                if (pfx & (1 << S6_GMAC_BURST_POSTRD_DROP)) {
+                        dev_kfree_skb_irq(skb);
+                } else {
+                        skb_put(skb, (pfx >> S6_GMAC_BURST_POSTRD_LEN)
+                                & S6_GMAC_BURST_POSTRD_LEN_MASK);
+                        skb->dev = dev;
+                        skb->protocol = eth_type_trans(skb, dev);
+                        skb->ip_summed = CHECKSUM_UNNECESSARY;
+                        netif_rx(skb);
+                }
+        }
+}
+
+static void s6gmac_tx_interrupt(struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        while (((u8)(pd->tx_skb_i - pd->tx_skb_o)) >
+                        s6dmac_pending_count(pd->tx_dma, pd->tx_chan)) {
+                dev_kfree_skb_irq(pd->tx_skb[(pd->tx_skb_o++) % S6_NUM_TX_SKB]);
+        }
+        if (!s6dmac_fifo_full(pd->tx_dma, pd->tx_chan))
+                netif_wake_queue(dev);
+}
+
+struct s6gmac_statinf {
+        unsigned reg_size : 4; /* 0: unused */
+        unsigned reg_off : 6;
+        unsigned net_index : 6;
+};
+
+#define S6_STATS_B (8 * sizeof(u32))
+#define S6_STATS_C(b, r, f) [b] = { \
+        BUILD_BUG_ON_ZERO(r##_SIZE < S6_GMAC_STAT_SIZE_MIN) + \
+        BUILD_BUG_ON_ZERO((r##_SIZE - (S6_GMAC_STAT_SIZE_MIN - 1)) \
+                        >= (1<<4)) + \
+        r##_SIZE - (S6_GMAC_STAT_SIZE_MIN - 1), \
+        BUILD_BUG_ON_ZERO(((unsigned)((r - S6_GMAC_STAT_REGS) / sizeof(u32))) \
+                        >= ((1<<6)-1)) + \
+        (r - S6_GMAC_STAT_REGS) / sizeof(u32), \
+        BUILD_BUG_ON_ZERO((offsetof(struct net_device_stats, f)) \
+                        % sizeof(unsigned long)) + \
+        BUILD_BUG_ON_ZERO((((unsigned)(offsetof(struct net_device_stats, f)) \
+                        / sizeof(unsigned long)) >= (1<<6))) + \
+        BUILD_BUG_ON_ZERO((sizeof(((struct net_device_stats *)0)->f) \
+                        != sizeof(unsigned long))) + \
+        (offsetof(struct net_device_stats, f)) / sizeof(unsigned long)},
+
+static const struct s6gmac_statinf statinf[2][S6_STATS_B] = { {
+        S6_STATS_C(S6_GMAC_STATCARRY1_RBYT, S6_GMAC_STATRBYT, rx_bytes)
+        S6_STATS_C(S6_GMAC_STATCARRY1_RPKT, S6_GMAC_STATRPKT, rx_packets)
+        S6_STATS_C(S6_GMAC_STATCARRY1_RFCS, S6_GMAC_STATRFCS, rx_crc_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY1_RMCA, S6_GMAC_STATRMCA, multicast)
+        S6_STATS_C(S6_GMAC_STATCARRY1_RALN, S6_GMAC_STATRALN, rx_frame_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY1_RFLR, S6_GMAC_STATRFLR, rx_length_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY1_RCDE, S6_GMAC_STATRCDE, rx_missed_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY1_RUND, S6_GMAC_STATRUND, rx_length_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY1_ROVR, S6_GMAC_STATROVR, rx_length_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY1_RFRG, S6_GMAC_STATRFRG, rx_crc_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY1_RJBR, S6_GMAC_STATRJBR, rx_crc_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY1_RDRP, S6_GMAC_STATRDRP, rx_dropped)
+}, {
+        S6_STATS_C(S6_GMAC_STATCARRY2_TBYT, S6_GMAC_STATTBYT, tx_bytes)
+        S6_STATS_C(S6_GMAC_STATCARRY2_TPKT, S6_GMAC_STATTPKT, tx_packets)
+        S6_STATS_C(S6_GMAC_STATCARRY2_TEDF, S6_GMAC_STATTEDF, tx_aborted_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY2_TXCL, S6_GMAC_STATTXCL, tx_aborted_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY2_TNCL, S6_GMAC_STATTNCL, collisions)
+        S6_STATS_C(S6_GMAC_STATCARRY2_TDRP, S6_GMAC_STATTDRP, tx_dropped)
+        S6_STATS_C(S6_GMAC_STATCARRY2_TJBR, S6_GMAC_STATTJBR, tx_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY2_TFCS, S6_GMAC_STATTFCS, tx_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY2_TOVR, S6_GMAC_STATTOVR, tx_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY2_TUND, S6_GMAC_STATTUND, tx_errors)
+        S6_STATS_C(S6_GMAC_STATCARRY2_TFRG, S6_GMAC_STATTFRG, tx_errors)
+} };
+
+static void s6gmac_stats_collect(struct s6gmac *pd,
+                const struct s6gmac_statinf *inf)
+{
+        int b;
+        for (b = 0; b < S6_STATS_B; b++) {
+                if (inf[b].reg_size) {
+                        pd->stats[inf[b].net_index] +=
+                                readl(pd->reg + S6_GMAC_STAT_REGS
+                                        + sizeof(u32) * inf[b].reg_off);
+                }
+        }
+}
+
+static void s6gmac_stats_carry(struct s6gmac *pd,
+                const struct s6gmac_statinf *inf, u32 mask)
+{
+        int b;
+        while (mask) {
+                b = fls(mask) - 1;
+                mask &= ~(1 << b);
+                pd->carry[inf[b].net_index] += (1 << inf[b].reg_size);
+        }
+}
+
+static inline u32 s6gmac_stats_pending(struct s6gmac *pd, int carry)
+{
+        int r = readl(pd->reg + S6_GMAC_STATCARRY(carry)) &
+                ~readl(pd->reg + S6_GMAC_STATCARRYMSK(carry));
+        return r;
+}
+
+static inline void s6gmac_stats_interrupt(struct s6gmac *pd, int carry)
+{
+        u32 mask;
+        mask = s6gmac_stats_pending(pd, carry);
+        if (mask) {
+                writel(mask, pd->reg + S6_GMAC_STATCARRY(carry));
+                s6gmac_stats_carry(pd, &statinf[carry][0], mask);
+        }
+}
+
+static irqreturn_t s6gmac_interrupt(int irq, void *dev_id)
+{
+        struct net_device *dev = (struct net_device *)dev_id;
+        struct s6gmac *pd = netdev_priv(dev);
+        if (!dev)
+                return IRQ_NONE;
+        spin_lock(&pd->lock);
+        if (s6dmac_termcnt_irq(pd->rx_dma, pd->rx_chan))
+                s6gmac_rx_interrupt(dev);
+        s6gmac_rx_fillfifo(pd);
+        if (s6dmac_termcnt_irq(pd->tx_dma, pd->tx_chan))
+                s6gmac_tx_interrupt(dev);
+        s6gmac_stats_interrupt(pd, 0);
+        s6gmac_stats_interrupt(pd, 1);
+        spin_unlock(&pd->lock);
+        return IRQ_HANDLED;
+}
+
+static inline void s6gmac_set_dstaddr(struct s6gmac *pd, int n,
+        u32 addrlo, u32 addrhi, u32 masklo, u32 maskhi)
+{
+        writel(addrlo, pd->reg + S6_GMAC_HOST_DSTADDRLO(n));
+        writel(addrhi, pd->reg + S6_GMAC_HOST_DSTADDRHI(n));
+        writel(masklo, pd->reg + S6_GMAC_HOST_DSTMASKLO(n));
+        writel(maskhi, pd->reg + S6_GMAC_HOST_DSTMASKHI(n));
+}
+
+static inline void s6gmac_stop_device(struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        writel(0, pd->reg + S6_GMAC_MACCONF1);
+}
+
+static inline void s6gmac_init_device(struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        int is_rgmii = !!(pd->phydev->supported
+                & (SUPPORTED_1000baseT_Full | SUPPORTED_1000baseT_Half));
+#if 0
+        writel(1 << S6_GMAC_MACCONF1_SYNCTX |
+                1 << S6_GMAC_MACCONF1_SYNCRX |
+                1 << S6_GMAC_MACCONF1_TXFLOWCTRL |
+                1 << S6_GMAC_MACCONF1_RXFLOWCTRL |
+                1 << S6_GMAC_MACCONF1_RESTXFUNC |
+                1 << S6_GMAC_MACCONF1_RESRXFUNC |
+                1 << S6_GMAC_MACCONF1_RESTXMACCTRL |
+                1 << S6_GMAC_MACCONF1_RESRXMACCTRL,
+                pd->reg + S6_GMAC_MACCONF1);
+#endif
+        writel(1 << S6_GMAC_MACCONF1_SOFTRES, pd->reg + S6_GMAC_MACCONF1);
+        udelay(1000);
+        writel(1 << S6_GMAC_MACCONF1_TXENA | 1 << S6_GMAC_MACCONF1_RXENA,
+                pd->reg + S6_GMAC_MACCONF1);
+        writel(1 << S6_GMAC_HOST_PBLKCTRL_TXSRES |
+                1 << S6_GMAC_HOST_PBLKCTRL_RXSRES,
+                pd->reg + S6_GMAC_HOST_PBLKCTRL);
+        writel(S6_GMAC_HOST_PBLKCTRL_SIZ_128 << S6_GMAC_HOST_PBLKCTRL_TXBSIZ |
+                S6_GMAC_HOST_PBLKCTRL_SIZ_128 << S6_GMAC_HOST_PBLKCTRL_RXBSIZ |
+                1 << S6_GMAC_HOST_PBLKCTRL_STATENA |
+                1 << S6_GMAC_HOST_PBLKCTRL_STATCLEAR |
+                is_rgmii << S6_GMAC_HOST_PBLKCTRL_RGMII,
+                pd->reg + S6_GMAC_HOST_PBLKCTRL);
+        writel(1 << S6_GMAC_MACCONF1_TXENA |
+                1 << S6_GMAC_MACCONF1_RXENA |
+                (dev->flags & IFF_LOOPBACK ? 1 : 0)
+                        << S6_GMAC_MACCONF1_LOOPBACK,
+                pd->reg + S6_GMAC_MACCONF1);
+        writel(dev->mtu && (dev->mtu < (S6_MAX_FRLEN - ETH_HLEN-ETH_FCS_LEN)) ?
+                        dev->mtu+ETH_HLEN+ETH_FCS_LEN : S6_MAX_FRLEN,
+                pd->reg + S6_GMAC_MACMAXFRAMELEN);
+        writel((pd->link.full ? 1 : 0) << S6_GMAC_MACCONF2_FULL |
+                1 << S6_GMAC_MACCONF2_PADCRCENA |
+                1 << S6_GMAC_MACCONF2_LENGTHFCHK |
+                (pd->link.giga ?
+                        S6_GMAC_MACCONF2_IFMODE_BYTE :
+                        S6_GMAC_MACCONF2_IFMODE_NIBBLE)
+                        << S6_GMAC_MACCONF2_IFMODE |
+                7 << S6_GMAC_MACCONF2_PREAMBLELEN,
+                pd->reg + S6_GMAC_MACCONF2);
+        writel(0, pd->reg + S6_GMAC_MACSTATADDR1);
+        writel(0, pd->reg + S6_GMAC_MACSTATADDR2);
+        writel(1 << S6_GMAC_FIFOCONF0_WTMENREQ |
+                1 << S6_GMAC_FIFOCONF0_SRFENREQ |
+                1 << S6_GMAC_FIFOCONF0_FRFENREQ |
+                1 << S6_GMAC_FIFOCONF0_STFENREQ |
+                1 << S6_GMAC_FIFOCONF0_FTFENREQ,
+                pd->reg + S6_GMAC_FIFOCONF0);
+        writel(128 << S6_GMAC_FIFOCONF3_CFGFTTH |
+                128 << S6_GMAC_FIFOCONF3_CFGHWMFT,
+                pd->reg + S6_GMAC_FIFOCONF3);
+        writel((S6_GMAC_FIFOCONF_RSV_MASK & ~(
+                        1 << S6_GMAC_FIFOCONF_RSV_RUNT |
+                        1 << S6_GMAC_FIFOCONF_RSV_CRCERR |
+                        1 << S6_GMAC_FIFOCONF_RSV_OK |
+                        1 << S6_GMAC_FIFOCONF_RSV_DRIBBLE |
+                        1 << S6_GMAC_FIFOCONF_RSV_CTRLFRAME |
+                        1 << S6_GMAC_FIFOCONF_RSV_PAUSECTRL |
+                        1 << S6_GMAC_FIFOCONF_RSV_UNOPCODE |
+                        1 << S6_GMAC_FIFOCONF_RSV_TRUNCATED)) |
+                1 << S6_GMAC_FIFOCONF5_DROPLT64 |
+                pd->link.giga << S6_GMAC_FIFOCONF5_CFGBYTM |
+                1 << S6_GMAC_FIFOCONF5_RXDROPSIZE,
+                pd->reg + S6_GMAC_FIFOCONF5);
+        writel(1 << S6_GMAC_FIFOCONF_RSV_RUNT |
+                1 << S6_GMAC_FIFOCONF_RSV_CRCERR |
+                1 << S6_GMAC_FIFOCONF_RSV_DRIBBLE |
+                1 << S6_GMAC_FIFOCONF_RSV_CTRLFRAME |
+                1 << S6_GMAC_FIFOCONF_RSV_PAUSECTRL |
+                1 << S6_GMAC_FIFOCONF_RSV_UNOPCODE |
+                1 << S6_GMAC_FIFOCONF_RSV_TRUNCATED,
+                pd->reg + S6_GMAC_FIFOCONF4);
+        s6gmac_set_dstaddr(pd, 0,
+                0xFFFFFFFF, 0x0000FFFF, 0xFFFFFFFF, 0x0000FFFF);
+        s6gmac_set_dstaddr(pd, 1,
+                dev->dev_addr[5] |
+                dev->dev_addr[4] << 8 |
+                dev->dev_addr[3] << 16 |
+                dev->dev_addr[2] << 24,
+                dev->dev_addr[1] |
+                dev->dev_addr[0] << 8,
+                0xFFFFFFFF, 0x0000FFFF);
+        s6gmac_set_dstaddr(pd, 2,
+                0x00000000, 0x00000100, 0x00000000, 0x00000100);
+        s6gmac_set_dstaddr(pd, 3,
+                0x00000000, 0x00000000, 0x00000000, 0x00000000);
+        writel(1 << S6_GMAC_HOST_PBLKCTRL_TXENA |
+                1 << S6_GMAC_HOST_PBLKCTRL_RXENA |
+                S6_GMAC_HOST_PBLKCTRL_SIZ_128 << S6_GMAC_HOST_PBLKCTRL_TXBSIZ |
+                S6_GMAC_HOST_PBLKCTRL_SIZ_128 << S6_GMAC_HOST_PBLKCTRL_RXBSIZ |
+                1 << S6_GMAC_HOST_PBLKCTRL_STATENA |
+                1 << S6_GMAC_HOST_PBLKCTRL_STATCLEAR |
+                is_rgmii << S6_GMAC_HOST_PBLKCTRL_RGMII,
+                pd->reg + S6_GMAC_HOST_PBLKCTRL);
+}
+
+static void s6mii_enable(struct s6gmac *pd)
+{
+        writel(readl(pd->reg + S6_GMAC_MACCONF1) &
+                ~(1 << S6_GMAC_MACCONF1_SOFTRES),
+                pd->reg + S6_GMAC_MACCONF1);
+        writel((readl(pd->reg + S6_GMAC_MACMIICONF)
+                & ~(S6_GMAC_MACMIICONF_CSEL_MASK << S6_GMAC_MACMIICONF_CSEL))
+                | (S6_GMAC_MACMIICONF_CSEL_DIV168 << S6_GMAC_MACMIICONF_CSEL),
+                pd->reg + S6_GMAC_MACMIICONF);
+}
+
+static int s6mii_busy(struct s6gmac *pd, int tmo)
+{
+        while (readl(pd->reg + S6_GMAC_MACMIIINDI)) {
+                if (--tmo == 0)
+                        return -ETIME;
+                udelay(64);
+        }
+        return 0;
+}
+
+static int s6mii_read(struct mii_bus *bus, int phy_addr, int regnum)
+{
+        struct s6gmac *pd = bus->priv;
+        s6mii_enable(pd);
+        if (s6mii_busy(pd, 256))
+                return -ETIME;
+        writel(phy_addr << S6_GMAC_MACMIIADDR_PHY |
+                regnum << S6_GMAC_MACMIIADDR_REG,
+                pd->reg + S6_GMAC_MACMIIADDR);
+        writel(1 << S6_GMAC_MACMIICMD_READ, pd->reg + S6_GMAC_MACMIICMD);
+        writel(0, pd->reg + S6_GMAC_MACMIICMD);
+        if (s6mii_busy(pd, 256))
+                return -ETIME;
+        return (u16)readl(pd->reg + S6_GMAC_MACMIISTAT);
+}
+
+static int s6mii_write(struct mii_bus *bus, int phy_addr, int regnum, u16 value)
+{
+        struct s6gmac *pd = bus->priv;
+        s6mii_enable(pd);
+        if (s6mii_busy(pd, 256))
+                return -ETIME;
+        writel(phy_addr << S6_GMAC_MACMIIADDR_PHY |
+                regnum << S6_GMAC_MACMIIADDR_REG,
+                pd->reg + S6_GMAC_MACMIIADDR);
+        writel(value, pd->reg + S6_GMAC_MACMIICTRL);
+        if (s6mii_busy(pd, 256))
+                return -ETIME;
+        return 0;
+}
+
+static int s6mii_reset(struct mii_bus *bus)
+{
+        struct s6gmac *pd = bus->priv;
+        s6mii_enable(pd);
+        if (s6mii_busy(pd, PHY_INIT_TIMEOUT))
+                return -ETIME;
+        return 0;
+}
+
+static void s6gmac_set_rgmii_txclock(struct s6gmac *pd)
+{
+        u32 pllsel = readl(S6_REG_GREG1 + S6_GREG1_PLLSEL);
+        pllsel &= ~(S6_GREG1_PLLSEL_GMAC_MASK << S6_GREG1_PLLSEL_GMAC);
+        switch (pd->link.mbit) {
+        case 10:
+                pllsel |= S6_GREG1_PLLSEL_GMAC_2500KHZ << S6_GREG1_PLLSEL_GMAC;
+                break;
+        case 100:
+                pllsel |= S6_GREG1_PLLSEL_GMAC_25MHZ << S6_GREG1_PLLSEL_GMAC;
+                break;
+        case 1000:
+                pllsel |= S6_GREG1_PLLSEL_GMAC_125MHZ << S6_GREG1_PLLSEL_GMAC;
+                break;
+        default:
+                return;
+        }
+        writel(pllsel, S6_REG_GREG1 + S6_GREG1_PLLSEL);
+}
+
+static inline void s6gmac_linkisup(struct net_device *dev, int isup)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        struct phy_device *phydev = pd->phydev;
+
+        pd->link.full = phydev->duplex;
+        pd->link.giga = (phydev->speed == 1000);
+        if (pd->link.mbit != phydev->speed) {
+                pd->link.mbit = phydev->speed;
+                s6gmac_set_rgmii_txclock(pd);
+        }
+        pd->link.isup = isup;
+        if (isup)
+                netif_carrier_on(dev);
+        phy_print_status(phydev);
+}
+
+static void s6gmac_adjust_link(struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        struct phy_device *phydev = pd->phydev;
+        if (pd->link.isup &&
+                        (!phydev->link ||
+                        (pd->link.mbit != phydev->speed) ||
+                        (pd->link.full != phydev->duplex))) {
+                pd->link.isup = 0;
+                netif_tx_disable(dev);
+                if (!phydev->link) {
+                        netif_carrier_off(dev);
+                        phy_print_status(phydev);
+                }
+        }
+        if (!pd->link.isup && phydev->link) {
+                if (pd->link.full != phydev->duplex) {
+                        u32 maccfg = readl(pd->reg + S6_GMAC_MACCONF2);
+                        if (phydev->duplex)
+                                maccfg |= 1 << S6_GMAC_MACCONF2_FULL;
+                        else
+                                maccfg &= ~(1 << S6_GMAC_MACCONF2_FULL);
+                        writel(maccfg, pd->reg + S6_GMAC_MACCONF2);
+                }
+
+                if (pd->link.giga != (phydev->speed == 1000)) {
+                        u32 fifocfg = readl(pd->reg + S6_GMAC_FIFOCONF5);
+                        u32 maccfg = readl(pd->reg + S6_GMAC_MACCONF2);
+                        maccfg &= ~(S6_GMAC_MACCONF2_IFMODE_MASK
+                                     << S6_GMAC_MACCONF2_IFMODE);
+                        if (phydev->speed == 1000) {
+                                fifocfg |= 1 << S6_GMAC_FIFOCONF5_CFGBYTM;
+                                maccfg |= S6_GMAC_MACCONF2_IFMODE_BYTE
+                                           << S6_GMAC_MACCONF2_IFMODE;
+                        } else {
+                                fifocfg &= ~(1 << S6_GMAC_FIFOCONF5_CFGBYTM);
+                                maccfg |= S6_GMAC_MACCONF2_IFMODE_NIBBLE
+                                           << S6_GMAC_MACCONF2_IFMODE;
+                        }
+                        writel(fifocfg, pd->reg + S6_GMAC_FIFOCONF5);
+                        writel(maccfg, pd->reg + S6_GMAC_MACCONF2);
+                }
+
+                if (!s6dmac_fifo_full(pd->tx_dma, pd->tx_chan))
+                        netif_wake_queue(dev);
+                s6gmac_linkisup(dev, 1);
+        }
+}
+
+static inline int s6gmac_phy_start(struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        int i = 0;
+        struct phy_device *p = NULL;
+        while ((!(p = pd->mii.bus->phy_map[i])) && (i < PHY_MAX_ADDR))
+                i++;
+        p = phy_connect(dev, dev_name(&p->dev), &s6gmac_adjust_link, 0,
+                        PHY_INTERFACE_MODE_RGMII);
+        if (IS_ERR(p)) {
+                printk(KERN_ERR "%s: Could not attach to PHY\n", dev->name);
+                return PTR_ERR(p);
+        }
+        p->supported &= PHY_GBIT_FEATURES;
+        p->advertising = p->supported;
+        pd->phydev = p;
+        return 0;
+}
+
+static inline void s6gmac_init_stats(struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        u32 mask;
+        mask =  1 << S6_GMAC_STATCARRY1_RDRP |
+                1 << S6_GMAC_STATCARRY1_RJBR |
+                1 << S6_GMAC_STATCARRY1_RFRG |
+                1 << S6_GMAC_STATCARRY1_ROVR |
+                1 << S6_GMAC_STATCARRY1_RUND |
+                1 << S6_GMAC_STATCARRY1_RCDE |
+                1 << S6_GMAC_STATCARRY1_RFLR |
+                1 << S6_GMAC_STATCARRY1_RALN |
+                1 << S6_GMAC_STATCARRY1_RMCA |
+                1 << S6_GMAC_STATCARRY1_RFCS |
+                1 << S6_GMAC_STATCARRY1_RPKT |
+                1 << S6_GMAC_STATCARRY1_RBYT;
+        writel(mask, pd->reg + S6_GMAC_STATCARRY(0));
+        writel(~mask, pd->reg + S6_GMAC_STATCARRYMSK(0));
+        mask =  1 << S6_GMAC_STATCARRY2_TDRP |
+                1 << S6_GMAC_STATCARRY2_TNCL |
+                1 << S6_GMAC_STATCARRY2_TXCL |
+                1 << S6_GMAC_STATCARRY2_TEDF |
+                1 << S6_GMAC_STATCARRY2_TPKT |
+                1 << S6_GMAC_STATCARRY2_TBYT |
+                1 << S6_GMAC_STATCARRY2_TFRG |
+                1 << S6_GMAC_STATCARRY2_TUND |
+                1 << S6_GMAC_STATCARRY2_TOVR |
+                1 << S6_GMAC_STATCARRY2_TFCS |
+                1 << S6_GMAC_STATCARRY2_TJBR;
+        writel(mask, pd->reg + S6_GMAC_STATCARRY(1));
+        writel(~mask, pd->reg + S6_GMAC_STATCARRYMSK(1));
+}
+
+static inline void s6gmac_init_dmac(struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        s6dmac_disable_chan(pd->tx_dma, pd->tx_chan);
+        s6dmac_disable_chan(pd->rx_dma, pd->rx_chan);
+        s6dmac_disable_error_irqs(pd->tx_dma, 1 << S6_HIFDMA_GMACTX);
+        s6dmac_disable_error_irqs(pd->rx_dma, 1 << S6_HIFDMA_GMACRX);
+}
+
+static int s6gmac_tx(struct sk_buff *skb, struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        unsigned long flags;
+        spin_lock_irqsave(&pd->lock, flags);
+        dev->trans_start = jiffies;
+        writel(skb->len << S6_GMAC_BURST_PREWR_LEN |
+                0 << S6_GMAC_BURST_PREWR_CFE |
+                1 << S6_GMAC_BURST_PREWR_PPE |
+                1 << S6_GMAC_BURST_PREWR_FCS |
+                ((skb->len < ETH_ZLEN) ? 1 : 0) << S6_GMAC_BURST_PREWR_PAD,
+                pd->reg + S6_GMAC_BURST_PREWR);
+        s6dmac_put_fifo_cache(pd->tx_dma, pd->tx_chan,
+                (u32)skb->data, pd->io, skb->len);
+        if (s6dmac_fifo_full(pd->tx_dma, pd->tx_chan))
+                netif_stop_queue(dev);
+        if (((u8)(pd->tx_skb_i - pd->tx_skb_o)) >= S6_NUM_TX_SKB) {
+                printk(KERN_ERR "GMAC BUG: skb tx ring overflow [%x, %x]\n",
+                        pd->tx_skb_o, pd->tx_skb_i);
+                BUG();
+        }
+        pd->tx_skb[(pd->tx_skb_i++) % S6_NUM_TX_SKB] = skb;
+        spin_unlock_irqrestore(&pd->lock, flags);
+        return 0;
+}
+
+static void s6gmac_tx_timeout(struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        unsigned long flags;
+        spin_lock_irqsave(&pd->lock, flags);
+        s6gmac_tx_interrupt(dev);
+        spin_unlock_irqrestore(&pd->lock, flags);
+}
+
+static int s6gmac_open(struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        unsigned long flags;
+        phy_read_status(pd->phydev);
+        spin_lock_irqsave(&pd->lock, flags);
+        pd->link.mbit = 0;
+        s6gmac_linkisup(dev, pd->phydev->link);
+        s6gmac_init_device(dev);
+        s6gmac_init_stats(dev);
+        s6gmac_init_dmac(dev);
+        s6gmac_rx_fillfifo(pd);
+        s6dmac_enable_chan(pd->rx_dma, pd->rx_chan,
+                2, 1, 0, 1, 0, 0, 0, 7, -1, 2, 0, 1);
+        s6dmac_enable_chan(pd->tx_dma, pd->tx_chan,
+                2, 0, 1, 0, 0, 0, 0, 7, -1, 2, 0, 1);
+        writel(0 << S6_GMAC_HOST_INT_TXBURSTOVER |
+                0 << S6_GMAC_HOST_INT_TXPREWOVER |
+                0 << S6_GMAC_HOST_INT_RXBURSTUNDER |
+                0 << S6_GMAC_HOST_INT_RXPOSTRFULL |
+                0 << S6_GMAC_HOST_INT_RXPOSTRUNDER,
+                pd->reg + S6_GMAC_HOST_INTMASK);
+        spin_unlock_irqrestore(&pd->lock, flags);
+        phy_start(pd->phydev);
+        netif_start_queue(dev);
+        return 0;
+}
+
+static int s6gmac_stop(struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        unsigned long flags;
+        netif_stop_queue(dev);
+        phy_stop(pd->phydev);
+        spin_lock_irqsave(&pd->lock, flags);
+        s6gmac_init_dmac(dev);
+        s6gmac_stop_device(dev);
+        while (pd->tx_skb_i != pd->tx_skb_o)
+                dev_kfree_skb(pd->tx_skb[(pd->tx_skb_o++) % S6_NUM_TX_SKB]);
+        while (pd->rx_skb_i != pd->rx_skb_o)
+                dev_kfree_skb(pd->rx_skb[(pd->rx_skb_o++) % S6_NUM_RX_SKB]);
+        spin_unlock_irqrestore(&pd->lock, flags);
+        return 0;
+}
+
+static struct net_device_stats *s6gmac_stats(struct net_device *dev)
+{
+        struct s6gmac *pd = netdev_priv(dev);
+        struct net_device_stats *st = (struct net_device_stats *)&pd->stats;
+        int i;
+        do {
+                unsigned long flags;
+                spin_lock_irqsave(&pd->lock, flags);
+                for (i = 0; i < sizeof(pd->stats) / sizeof(unsigned long); i++)
+                        pd->stats[i] =
+                                pd->carry[i] << (S6_GMAC_STAT_SIZE_MIN - 1);
+                s6gmac_stats_collect(pd, &statinf[0][0]);
+                s6gmac_stats_collect(pd, &statinf[1][0]);
+                i = s6gmac_stats_pending(pd, 0) |
+                        s6gmac_stats_pending(pd, 1);
+                spin_unlock_irqrestore(&pd->lock, flags);
+        } while (i);
+        st->rx_errors = st->rx_crc_errors +
+                        st->rx_frame_errors +
+                        st->rx_length_errors +
+                        st->rx_missed_errors;
+        st->tx_errors += st->tx_aborted_errors;
+        return st;
+}
+
+static int __devinit s6gmac_probe(struct platform_device *pdev)
+{
+        struct net_device *dev;
+        struct s6gmac *pd;
+        int res;
+        unsigned long i;
+        struct mii_bus *mb;
+        dev = alloc_etherdev(sizeof(*pd));
+        if (!dev) {
+                printk(KERN_ERR DRV_PRMT "etherdev alloc failed, aborting.\n");
+                return -ENOMEM;
+        }
+        dev->open = s6gmac_open;
+        dev->stop = s6gmac_stop;
+        dev->hard_start_xmit = s6gmac_tx;
+        dev->tx_timeout = s6gmac_tx_timeout;
+        dev->watchdog_timeo = HZ;
+        dev->get_stats = s6gmac_stats;
+        dev->irq = platform_get_irq(pdev, 0);
+        pd = netdev_priv(dev);
+        memset(pd, 0, sizeof(*pd));
+        spin_lock_init(&pd->lock);
+        pd->reg = platform_get_resource(pdev, IORESOURCE_MEM, 0)->start;
+        i = platform_get_resource(pdev, IORESOURCE_DMA, 0)->start;
+        pd->tx_dma = DMA_MASK_DMAC(i);
+        pd->tx_chan = DMA_INDEX_CHNL(i);
+        i = platform_get_resource(pdev, IORESOURCE_DMA, 1)->start;
+        pd->rx_dma = DMA_MASK_DMAC(i);
+        pd->rx_chan = DMA_INDEX_CHNL(i);
+        pd->io = platform_get_resource(pdev, IORESOURCE_IO, 0)->start;
+        res = request_irq(dev->irq, &s6gmac_interrupt, 0, dev->name, dev);
+        if (res) {
+                printk(KERN_ERR DRV_PRMT "irq request failed: %d\n", dev->irq);
+                goto errirq;
+        }
+        res = register_netdev(dev);
+        if (res) {
+                printk(KERN_ERR DRV_PRMT "error registering device %s\n",
+                        dev->name);
+                goto errdev;
+        }
+        mb = mdiobus_alloc();
+        if (!mb) {
+                printk(KERN_ERR DRV_PRMT "error allocating mii bus\n");
+                goto errmii;
+        }
+        mb->name = "s6gmac_mii";
+        mb->read = s6mii_read;
+        mb->write = s6mii_write;
+        mb->reset = s6mii_reset;
+        mb->priv = pd;
+        snprintf(mb->id, MII_BUS_ID_SIZE, "0");
+        mb->phy_mask = ~(1 << 0);
+        mb->irq = &pd->mii.irq[0];
+        for (i = 0; i < PHY_MAX_ADDR; i++) {
+                int n = platform_get_irq(pdev, i + 1);
+                if (n < 0)
+                        n = PHY_POLL;
+                pd->mii.irq[i] = n;
+        }
+        mdiobus_register(mb);
+        pd->mii.bus = mb;
+        res = s6gmac_phy_start(dev);
+        if (res)
+                return res;
+        platform_set_drvdata(pdev, dev);
+        return 0;
+errmii:
+        unregister_netdev(dev);
+errdev:
+        free_irq(dev->irq, dev);
+errirq:
+        free_netdev(dev);
+        return res;
+}
+
+static int __devexit s6gmac_remove(struct platform_device *pdev)
+{
+        struct net_device *dev = platform_get_drvdata(pdev);
+        if (dev) {
+                struct s6gmac *pd = netdev_priv(dev);
+                mdiobus_unregister(pd->mii.bus);
+                unregister_netdev(dev);
+                free_irq(dev->irq, dev);
+                free_netdev(dev);
+                platform_set_drvdata(pdev, NULL);
+        }
+        return 0;
+}
+
+static struct platform_driver s6gmac_driver = {
+        .probe = s6gmac_probe,
+        .remove = __devexit_p(s6gmac_remove),
+        .driver = {
+                .name = "s6gmac",
+                .owner = THIS_MODULE,
+        },
+};
+
+static int __init s6gmac_init(void)
+{
+        printk(KERN_INFO DRV_PRMT "S6 GMAC ethernet driver\n");
+        return platform_driver_register(&s6gmac_driver);
+}
+
+
+static void __exit s6gmac_exit(void)
+{
+        platform_driver_unregister(&s6gmac_driver);
+}
+
+module_init(s6gmac_init);
+module_exit(s6gmac_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("S6105 on chip Ethernet driver");
+MODULE_AUTHOR("Oskar Schirmer <os@emlix.com>");
diff --git a/drivers/net/usb/Kconfig b/drivers/net/usb/Kconfig
index 3717569828bf..a906d3998131 100644
--- a/drivers/net/usb/Kconfig
+++ b/drivers/net/usb/Kconfig
@@ -169,10 +169,12 @@ config USB_NET_CDCETHER
           The Linux-USB CDC Ethernet Gadget driver is an open implementation.
           This driver should work with at least the following devices:
 
+            * Dell Wireless 5530 HSPA
             * Ericsson PipeRider (all variants)
+            * Ericsson Mobile Broadband Module (all variants)
             * Motorola (DM100 and SB4100)
             * Broadcom Cable Modem (reference design)
-            * Toshiba PCX1100U
+            * Toshiba (PCX1100U and F3507g)
             * ...
 
           This driver creates an interface named "ethX", where X depends on
diff --git a/drivers/net/usb/cdc_ether.c b/drivers/net/usb/cdc_ether.c
index 01fd528306ec..4a6aff579403 100644
--- a/drivers/net/usb/cdc_ether.c
+++ b/drivers/net/usb/cdc_ether.c
@@ -533,6 +533,31 @@ static const struct usb_device_id	products [] = {
         USB_DEVICE_AND_INTERFACE_INFO(0x0bdb, 0x1900, USB_CLASS_COMM,
                         USB_CDC_SUBCLASS_MDLM, USB_CDC_PROTO_NONE),
         .driver_info = (unsigned long) &cdc_info,
+}, {
+        /* Ericsson F3507g ver. 2 */
+        USB_DEVICE_AND_INTERFACE_INFO(0x0bdb, 0x1902, USB_CLASS_COMM,
+                        USB_CDC_SUBCLASS_MDLM, USB_CDC_PROTO_NONE),
+        .driver_info = (unsigned long) &cdc_info,
+}, {
+        /* Ericsson F3607gw */
+        USB_DEVICE_AND_INTERFACE_INFO(0x0bdb, 0x1904, USB_CLASS_COMM,
+                        USB_CDC_SUBCLASS_MDLM, USB_CDC_PROTO_NONE),
+        .driver_info = (unsigned long) &cdc_info,
+}, {
+        /* Ericsson F3307 */
+        USB_DEVICE_AND_INTERFACE_INFO(0x0bdb, 0x1906, USB_CLASS_COMM,
+                        USB_CDC_SUBCLASS_MDLM, USB_CDC_PROTO_NONE),
+        .driver_info = (unsigned long) &cdc_info,
+}, {
+        /* Toshiba F3507g */
+        USB_DEVICE_AND_INTERFACE_INFO(0x0930, 0x130b, USB_CLASS_COMM,
+                        USB_CDC_SUBCLASS_MDLM, USB_CDC_PROTO_NONE),
+        .driver_info = (unsigned long) &cdc_info,
+}, {
+        /* Dell F3507g */
+        USB_DEVICE_AND_INTERFACE_INFO(0x413c, 0x8147, USB_CLASS_COMM,
+                        USB_CDC_SUBCLASS_MDLM, USB_CDC_PROTO_NONE),
+        .driver_info = (unsigned long) &cdc_info,
 },
         { },            // END
 };
diff --git a/drivers/net/usb/cdc_subset.c b/drivers/net/usb/cdc_subset.c
index c66b9c324f54..ca39ace0b0eb 100644
--- a/drivers/net/usb/cdc_subset.c
+++ b/drivers/net/usb/cdc_subset.c
@@ -307,9 +307,10 @@ static const struct usb_device_id	products [] = {
         USB_DEVICE (0x1286, 0x8001),    // "blob" bootloader
         .driver_info =  (unsigned long) &blob_info,
 }, {
-        // Linux Ethernet/RNDIS gadget on pxa210/25x/26x, second config
-        // e.g. Gumstix, current OpenZaurus, ...
-        USB_DEVICE_VER (0x0525, 0xa4a2, 0x0203, 0x0203),
+        // Linux Ethernet/RNDIS gadget, mostly on PXA, second config
+        // e.g. Gumstix, current OpenZaurus, ... or anything else
+        // that just enables this gadget option.
+        USB_DEVICE (0x0525, 0xa4a2),
         .driver_info =  (unsigned long) &linuxdev_info,
 },
 #endif
diff --git a/drivers/net/usb/pegasus.c b/drivers/net/usb/pegasus.c
index 2138535f2339..73acbd244aa1 100644
--- a/drivers/net/usb/pegasus.c
+++ b/drivers/net/usb/pegasus.c
@@ -297,7 +297,7 @@ static int update_eth_regs_async(pegasus_t * pegasus)
 
         pegasus->dr.bRequestType = PEGASUS_REQT_WRITE;
         pegasus->dr.bRequest = PEGASUS_REQ_SET_REGS;
-        pegasus->dr.wValue = 0;
+        pegasus->dr.wValue = cpu_to_le16(0);
         pegasus->dr.wIndex = cpu_to_le16(EthCtrl0);
         pegasus->dr.wLength = cpu_to_le16(3);
         pegasus->ctrl_urb->transfer_buffer_length = 3;
@@ -446,11 +446,12 @@ static int write_eprom_word(pegasus_t * pegasus, __u8 index, __u16 data)
         int i;
         __u8 tmp, d[4] = { 0x3f, 0, 0, EPROM_WRITE };
         int ret;
+        __le16 le_data = cpu_to_le16(data);
 
         set_registers(pegasus, EpromOffset, 4, d);
         enable_eprom_write(pegasus);
         set_register(pegasus, EpromOffset, index);
-        set_registers(pegasus, EpromData, 2, &data);
+        set_registers(pegasus, EpromData, 2, &le_data);
         set_register(pegasus, EpromCtrl, EPROM_WRITE);
 
         for (i = 0; i < REG_TIMEOUT; i++) {
@@ -923,29 +924,32 @@ static struct net_device_stats *pegasus_netdev_stats(struct net_device *dev)
 
 static inline void disable_net_traffic(pegasus_t * pegasus)
 {
-        int tmp = 0;
+        __le16 tmp = cpu_to_le16(0);
 
-        set_registers(pegasus, EthCtrl0, 2, &tmp);
+        set_registers(pegasus, EthCtrl0, sizeof(tmp), &tmp);
 }
 
 static inline void get_interrupt_interval(pegasus_t * pegasus)
 {
-        __u8 data[2];
+        u16 data;
+        u8 interval;
 
-        read_eprom_word(pegasus, 4, (__u16 *) data);
+        read_eprom_word(pegasus, 4, &data);
+        interval = data >> 8;
         if (pegasus->usb->speed != USB_SPEED_HIGH) {
-                if (data[1] < 0x80) {
+                if (interval < 0x80) {
                         if (netif_msg_timer(pegasus))
                                 dev_info(&pegasus->intf->dev, "intr interval "
                                         "changed from %ums to %ums\n",
-                                        data[1], 0x80);
-                        data[1] = 0x80;
+                                        interval, 0x80);
+                        interval = 0x80;
+                        data = (data & 0x00FF) | ((u16)interval << 8);
 #ifdef PEGASUS_WRITE_EEPROM
-                        write_eprom_word(pegasus, 4, *(__u16 *) data);
+                        write_eprom_word(pegasus, 4, data);
 #endif
                 }
         }
-        pegasus->intr_interval = data[1];
+        pegasus->intr_interval = interval;
 }
 
 static void set_carrier(struct net_device *net)
@@ -1299,7 +1303,8 @@ static int pegasus_blacklisted(struct usb_device *udev)
         /* Special quirk to keep the driver from handling the Belkin Bluetooth
          * dongle which happens to have the same ID.
          */
-        if ((udd->idVendor == VENDOR_BELKIN && udd->idProduct == 0x0121) &&
+        if ((udd->idVendor == cpu_to_le16(VENDOR_BELKIN)) &&
+            (udd->idProduct == cpu_to_le16(0x0121)) &&
             (udd->bDeviceClass == USB_CLASS_WIRELESS_CONTROLLER) &&
             (udd->bDeviceProtocol == 1))
                 return 1;
diff --git a/drivers/net/via-velocity.c b/drivers/net/via-velocity.c
index b02f7adff5dc..3ba35956327a 100644
--- a/drivers/net/via-velocity.c
+++ b/drivers/net/via-velocity.c
@@ -1847,7 +1847,7 @@ static void velocity_free_tx_buf(struct velocity_info *vptr, struct velocity_td_
          */
         if (tdinfo->skb_dma) {
 
-                pktlen = (skb->len > ETH_ZLEN ? : ETH_ZLEN);
+                pktlen = max_t(unsigned int, skb->len, ETH_ZLEN);
                 for (i = 0; i < tdinfo->nskb_dma; i++) {
 #ifdef VELOCITY_ZERO_COPY_SUPPORT
                         pci_unmap_single(vptr->pdev, tdinfo->skb_dma[i], le16_to_cpu(td->tdesc1.len), PCI_DMA_TODEVICE);
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index 55f7de09d134..ea045151f953 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -538,6 +538,7 @@ ath5k_pci_probe(struct pci_dev *pdev,
         sc->iobase = mem; /* So we can unmap it on detach */
         sc->cachelsz = csz * sizeof(u32); /* convert to bytes */
         sc->opmode = NL80211_IFTYPE_STATION;
+        sc->bintval = 1000;
         mutex_init(&sc->lock);
         spin_lock_init(&sc->rxbuflock);
         spin_lock_init(&sc->txbuflock);
@@ -686,6 +687,13 @@ ath5k_pci_resume(struct pci_dev *pdev)
         if (err)
                 return err;
 
+        /*
+         * Suspend/Resume resets the PCI configuration space, so we have to
+         * re-disable the RETRY_TIMEOUT register (0x41) to keep
+         * PCI Tx retries from interfering with C3 CPU state
+         */
+        pci_write_config_byte(pdev, 0x41, 0);
+
         err = request_irq(pdev->irq, ath5k_intr, IRQF_SHARED, "ath", sc);
         if (err) {
                 ATH5K_ERR(sc, "request_irq failed\n");
@@ -2748,9 +2756,6 @@ static int ath5k_add_interface(struct ieee80211_hw *hw,
                 goto end;
         }
 
-        /* Set to a reasonable value. Note that this will
-         * be set to mac80211's value at ath5k_config(). */
-        sc->bintval = 1000;
         ath5k_hw_set_lladdr(sc->ah, conf->mac_addr);
 
         ret = 0;
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 9f49a3251d4d..66a6c1f5022a 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -1196,8 +1196,8 @@ void ath_radio_disable(struct ath_softc *sc)
 
         ath9k_hw_phy_disable(ah);
         ath9k_hw_configpcipowersave(ah, 1);
-        ath9k_hw_setpower(ah, ATH9K_PM_FULL_SLEEP);
         ath9k_ps_restore(sc);
+        ath9k_hw_setpower(ah, ATH9K_PM_FULL_SLEEP);
 }
 
 /*******************/
diff --git a/drivers/net/wireless/ath/ath9k/pci.c b/drivers/net/wireless/ath/ath9k/pci.c
index ccdf20a2e9be..170c5b32e49b 100644
--- a/drivers/net/wireless/ath/ath9k/pci.c
+++ b/drivers/net/wireless/ath/ath9k/pci.c
@@ -87,6 +87,7 @@ static int ath_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
         struct ath_softc *sc;
         struct ieee80211_hw *hw;
         u8 csz;
+        u32 val;
         int ret = 0;
         struct ath_hw *ah;
 
@@ -133,6 +134,14 @@ static int ath_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
         pci_set_master(pdev);
 
+        /*
+         * Disable the RETRY_TIMEOUT register (0x41) to keep
+         * PCI Tx retries from interfering with C3 CPU state.
+         */
+        pci_read_config_dword(pdev, 0x40, &val);
+        if ((val & 0x0000ff00) != 0)
+                pci_write_config_dword(pdev, 0x40, val & 0xffff00ff);
+
         ret = pci_request_region(pdev, 0, "ath9k");
         if (ret) {
                 dev_err(&pdev->dev, "PCI memory region reserve error\n");
@@ -239,12 +248,21 @@ static int ath_pci_resume(struct pci_dev *pdev)
         struct ieee80211_hw *hw = pci_get_drvdata(pdev);
         struct ath_wiphy *aphy = hw->priv;
         struct ath_softc *sc = aphy->sc;
+        u32 val;
         int err;
 
         err = pci_enable_device(pdev);
         if (err)
                 return err;
         pci_restore_state(pdev);
+        /*
+         * Suspend/Resume resets the PCI configuration space, so we have to
+         * re-disable the RETRY_TIMEOUT register (0x41) to keep
+         * PCI Tx retries from interfering with C3 CPU state
+         */
+        pci_read_config_dword(pdev, 0x40, &val);
+        if ((val & 0x0000ff00) != 0)
+                pci_write_config_dword(pdev, 0x40, val & 0xffff00ff);
 
         /* Enable LED */
         ath9k_hw_cfg_output(sc->sc_ah, ATH_LED_PIN,
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index f99f3a76df3f..cece1c4c6bda 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -539,11 +539,14 @@ static void ath_rx_ps_beacon(struct ath_softc *sc, struct sk_buff *skb)
         if (ath_beacon_dtim_pending_cab(skb)) {
                 /*
                  * Remain awake waiting for buffered broadcast/multicast
-                 * frames.
+                 * frames. If the last broadcast/multicast frame is not
+                 * received properly, the next beacon frame will work as
+                 * a backup trigger for returning into NETWORK SLEEP state,
+                 * so we are waiting for it as well.
                  */
                 DPRINTF(sc, ATH_DBG_PS, "Received DTIM beacon indicating "
                         "buffered broadcast/multicast frame(s)\n");
-                sc->sc_flags |= SC_OP_WAIT_FOR_CAB;
+                sc->sc_flags |= SC_OP_WAIT_FOR_CAB | SC_OP_WAIT_FOR_BEACON;
                 return;
         }
 
diff --git a/drivers/net/wireless/iwmc3200wifi/iwm.h b/drivers/net/wireless/iwmc3200wifi/iwm.h
index 635c16ee6186..77c339f8516c 100644
--- a/drivers/net/wireless/iwmc3200wifi/iwm.h
+++ b/drivers/net/wireless/iwmc3200wifi/iwm.h
@@ -288,6 +288,7 @@ struct iwm_priv {
         u8 *eeprom;
         struct timer_list watchdog;
         struct work_struct reset_worker;
+        struct mutex mutex;
         struct rfkill *rfkill;
 
         char private[0] __attribute__((__aligned__(NETDEV_ALIGN)));
@@ -315,8 +316,11 @@ extern const struct iw_handler_def iwm_iw_handler_def;
 void *iwm_if_alloc(int sizeof_bus, struct device *dev,
                    struct iwm_if_ops *if_ops);
 void iwm_if_free(struct iwm_priv *iwm);
+int iwm_if_add(struct iwm_priv *iwm);
+void iwm_if_remove(struct iwm_priv *iwm);
 int iwm_mode_to_nl80211_iftype(int mode);
 int iwm_priv_init(struct iwm_priv *iwm);
+void iwm_priv_deinit(struct iwm_priv *iwm);
 void iwm_reset(struct iwm_priv *iwm);
 void iwm_tx_credit_init_pools(struct iwm_priv *iwm,
                               struct iwm_umac_notif_alive *alive);
diff --git a/drivers/net/wireless/iwmc3200wifi/main.c b/drivers/net/wireless/iwmc3200wifi/main.c
index 6a2640f16b6d..8be206d58222 100644
--- a/drivers/net/wireless/iwmc3200wifi/main.c
+++ b/drivers/net/wireless/iwmc3200wifi/main.c
@@ -112,6 +112,9 @@ static void iwm_statistics_request(struct work_struct *work)
         iwm_send_umac_stats_req(iwm, 0);
 }
 
+int __iwm_up(struct iwm_priv *iwm);
+int __iwm_down(struct iwm_priv *iwm);
+
 static void iwm_reset_worker(struct work_struct *work)
 {
         struct iwm_priv *iwm;
@@ -120,6 +123,19 @@ static void iwm_reset_worker(struct work_struct *work)
 
         iwm = container_of(work, struct iwm_priv, reset_worker);
 
+        /*
+         * XXX: The iwm->mutex is introduced purely for this reset work,
+         * because the other users for iwm_up and iwm_down are only netdev
+         * ndo_open and ndo_stop which are already protected by rtnl.
+         * Please remove iwm->mutex together if iwm_reset_worker() is not
+         * required in the future.
+         */
+        if (!mutex_trylock(&iwm->mutex)) {
+                IWM_WARN(iwm, "We are in the middle of interface bringing "
+                         "UP/DOWN. Skip driver resetting.\n");
+                return;
+        }
+
         if (iwm->umac_profile_active) {
                 profile = kmalloc(sizeof(struct iwm_umac_profile), GFP_KERNEL);
                 if (profile)
@@ -128,10 +144,10 @@ static void iwm_reset_worker(struct work_struct *work)
                         IWM_ERR(iwm, "Couldn't alloc memory for profile\n");
         }
 
-        iwm_down(iwm);
+        __iwm_down(iwm);
 
         while (retry++ < 3) {
-                ret = iwm_up(iwm);
+                ret = __iwm_up(iwm);
                 if (!ret)
                         break;
 
@@ -142,7 +158,7 @@ static void iwm_reset_worker(struct work_struct *work)
                 IWM_WARN(iwm, "iwm_up() failed: %d\n", ret);
 
                 kfree(profile);
-                return;
+                goto out;
         }
 
         if (profile) {
@@ -151,6 +167,9 @@ static void iwm_reset_worker(struct work_struct *work)
                 iwm_send_mlme_profile(iwm);
                 kfree(profile);
         }
+
+ out:
+        mutex_unlock(&iwm->mutex);
 }
 
 static void iwm_watchdog(unsigned long data)
@@ -215,10 +234,21 @@ int iwm_priv_init(struct iwm_priv *iwm)
         init_timer(&iwm->watchdog);
         iwm->watchdog.function = iwm_watchdog;
         iwm->watchdog.data = (unsigned long)iwm;
+        mutex_init(&iwm->mutex);
 
         return 0;
 }
 
+void iwm_priv_deinit(struct iwm_priv *iwm)
+{
+        int i;
+
+        for (i = 0; i < IWM_TX_QUEUES; i++)
+                destroy_workqueue(iwm->txq[i].wq);
+
+        destroy_workqueue(iwm->rx_wq);
+}
+
 /*
  * We reset all the structures, and we reset the UMAC.
  * After calling this routine, you're expected to reload
@@ -466,7 +496,7 @@ void iwm_link_off(struct iwm_priv *iwm)
 
         iwm_rx_free(iwm);
 
-        cancel_delayed_work(&iwm->stats_request);
+        cancel_delayed_work_sync(&iwm->stats_request);
         memset(wstats, 0, sizeof(struct iw_statistics));
         wstats->qual.updated = IW_QUAL_ALL_INVALID;
 
@@ -511,7 +541,7 @@ static int iwm_channels_init(struct iwm_priv *iwm)
         return 0;
 }
 
-int iwm_up(struct iwm_priv *iwm)
+int __iwm_up(struct iwm_priv *iwm)
 {
         int ret;
         struct iwm_notif *notif_reboot, *notif_ack = NULL;
@@ -647,7 +677,18 @@ int iwm_up(struct iwm_priv *iwm)
         return -EIO;
 }
 
-int iwm_down(struct iwm_priv *iwm)
+int iwm_up(struct iwm_priv *iwm)
+{
+        int ret;
+
+        mutex_lock(&iwm->mutex);
+        ret = __iwm_up(iwm);
+        mutex_unlock(&iwm->mutex);
+
+        return ret;
+}
+
+int __iwm_down(struct iwm_priv *iwm)
 {
         int ret;
 
@@ -678,3 +719,14 @@ int iwm_down(struct iwm_priv *iwm)
 
         return 0;
 }
+
+int iwm_down(struct iwm_priv *iwm)
+{
+        int ret;
+
+        mutex_lock(&iwm->mutex);
+        ret = __iwm_down(iwm);
+        mutex_unlock(&iwm->mutex);
+
+        return ret;
+}
diff --git a/drivers/net/wireless/iwmc3200wifi/netdev.c b/drivers/net/wireless/iwmc3200wifi/netdev.c
index 68e2c3b6c7a1..aaa20c6885c8 100644
--- a/drivers/net/wireless/iwmc3200wifi/netdev.c
+++ b/drivers/net/wireless/iwmc3200wifi/netdev.c
@@ -114,32 +114,31 @@ void *iwm_if_alloc(int sizeof_bus, struct device *dev,
         iwm = wdev_to_iwm(wdev);
         iwm->bus_ops = if_ops;
         iwm->wdev = wdev;
-        iwm_priv_init(iwm);
+
+        ret = iwm_priv_init(iwm);
+        if (ret) {
+                dev_err(dev, "failed to init iwm_priv\n");
+                goto out_wdev;
+        }
+
         wdev->iftype = iwm_mode_to_nl80211_iftype(iwm->conf.mode);
 
-        ndev = alloc_netdev_mq(0, "wlan%d", ether_setup,
-                               IWM_TX_QUEUES);
+        ndev = alloc_netdev_mq(0, "wlan%d", ether_setup, IWM_TX_QUEUES);
         if (!ndev) {
                 dev_err(dev, "no memory for network device instance\n");
-                goto out_wdev;
+                goto out_priv;
         }
 
         ndev->netdev_ops = &iwm_netdev_ops;
         ndev->wireless_handlers = &iwm_iw_handler_def;
         ndev->ieee80211_ptr = wdev;
         SET_NETDEV_DEV(ndev, wiphy_dev(wdev->wiphy));
-        ret = register_netdev(ndev);
-        if (ret < 0) {
-                dev_err(dev, "Failed to register netdev: %d\n", ret);
-                goto out_ndev;
-        }
-
         wdev->netdev = ndev;
 
         return iwm;
 
- out_ndev:
-        free_netdev(ndev);
+ out_priv:
+        iwm_priv_deinit(iwm);
 
  out_wdev:
         iwm_wdev_free(iwm);
@@ -148,15 +147,29 @@ void *iwm_if_alloc(int sizeof_bus, struct device *dev,
 
 void iwm_if_free(struct iwm_priv *iwm)
 {
-        int i;
-
         if (!iwm_to_ndev(iwm))
                 return;
 
-        unregister_netdev(iwm_to_ndev(iwm));
         free_netdev(iwm_to_ndev(iwm));
         iwm_wdev_free(iwm);
-        destroy_workqueue(iwm->rx_wq);
-        for (i = 0; i < IWM_TX_QUEUES; i++)
-                destroy_workqueue(iwm->txq[i].wq);
+        iwm_priv_deinit(iwm);
+}
+
+int iwm_if_add(struct iwm_priv *iwm)
+{
+        struct net_device *ndev = iwm_to_ndev(iwm);
+        int ret;
+
+        ret = register_netdev(ndev);
+        if (ret < 0) {
+                dev_err(&ndev->dev, "Failed to register netdev: %d\n", ret);
+                return ret;
+        }
+
+        return 0;
+}
+
+void iwm_if_remove(struct iwm_priv *iwm)
+{
+        unregister_netdev(iwm_to_ndev(iwm));
 }
diff --git a/drivers/net/wireless/iwmc3200wifi/sdio.c b/drivers/net/wireless/iwmc3200wifi/sdio.c
index b54da677b371..916681837fd2 100644
--- a/drivers/net/wireless/iwmc3200wifi/sdio.c
+++ b/drivers/net/wireless/iwmc3200wifi/sdio.c
@@ -454,10 +454,18 @@ static int iwm_sdio_probe(struct sdio_func *func,
 
         INIT_WORK(&hw->isr_worker, iwm_sdio_isr_worker);
 
+        ret = iwm_if_add(iwm);
+        if (ret) {
+                dev_err(dev, "add SDIO interface failed\n");
+                goto destroy_wq;
+        }
+
         dev_info(dev, "IWM SDIO probe\n");
 
         return 0;
 
+ destroy_wq:
+        destroy_workqueue(hw->isr_wq);
  debugfs_exit:
         iwm_debugfs_exit(iwm);
  if_free:
@@ -471,9 +479,10 @@ static void iwm_sdio_remove(struct sdio_func *func)
         struct iwm_priv *iwm = hw_to_iwm(hw);
         struct device *dev = &func->dev;
 
+        iwm_if_remove(iwm);
+        destroy_workqueue(hw->isr_wq);
         iwm_debugfs_exit(iwm);
         iwm_if_free(iwm);
-        destroy_workqueue(hw->isr_wq);
 
         sdio_set_drvdata(func, NULL);
 
diff --git a/drivers/net/wireless/zd1211rw/zd_usb.c b/drivers/net/wireless/zd1211rw/zd_usb.c
index f0e5e943f6e3..14a19baff214 100644
--- a/drivers/net/wireless/zd1211rw/zd_usb.c
+++ b/drivers/net/wireless/zd1211rw/zd_usb.c
@@ -67,6 +67,7 @@ static struct usb_device_id usb_ids[] = {
         { USB_DEVICE(0x079b, 0x0062), .driver_info = DEVICE_ZD1211B },
         { USB_DEVICE(0x1582, 0x6003), .driver_info = DEVICE_ZD1211B },
         { USB_DEVICE(0x050d, 0x705c), .driver_info = DEVICE_ZD1211B },
+        { USB_DEVICE(0x083a, 0xe503), .driver_info = DEVICE_ZD1211B },
         { USB_DEVICE(0x083a, 0xe506), .driver_info = DEVICE_ZD1211B },
         { USB_DEVICE(0x083a, 0x4505), .driver_info = DEVICE_ZD1211B },
         { USB_DEVICE(0x0471, 0x1236), .driver_info = DEVICE_ZD1211B },
diff --git a/drivers/parport/parport_pc.c b/drivers/parport/parport_pc.c
index 151bf5bc8afe..1032d5fdbd42 100644
--- a/drivers/parport/parport_pc.c
+++ b/drivers/parport/parport_pc.c
@@ -1471,11 +1471,13 @@ static void __devinit decode_smsc(int efer, int key, int devid, int devrev)
 
 static void __devinit winbond_check(int io, int key)
 {
-        int devid, devrev, oldid, x_devid, x_devrev, x_oldid;
+        int origval, devid, devrev, oldid, x_devid, x_devrev, x_oldid;
 
         if (!request_region(io, 3, __func__))
                 return;
 
+        origval = inb(io); /* Save original value */
+
         /* First probe without key */
         outb(0x20, io);
         x_devid = inb(io + 1);
@@ -1495,6 +1497,8 @@ static void __devinit winbond_check(int io, int key)
         oldid = inb(io + 1);
         outb(0xaa, io);    /* Magic Seal */
 
+        outb(origval, io); /* in case we poked some entirely different hardware */
+
         if ((x_devid == devid) && (x_devrev == devrev) && (x_oldid == oldid))
                 goto out; /* protection against false positives */
 
@@ -1505,11 +1509,15 @@ out:
 
 static void __devinit winbond_check2(int io, int key)
 {
-        int devid, devrev, oldid, x_devid, x_devrev, x_oldid;
+        int origval[3], devid, devrev, oldid, x_devid, x_devrev, x_oldid;
 
         if (!request_region(io, 3, __func__))
                 return;
 
+        origval[0] = inb(io); /* Save original values */
+        origval[1] = inb(io + 1);
+        origval[2] = inb(io + 2);
+
         /* First probe without the key */
         outb(0x20, io + 2);
         x_devid = inb(io + 2);
@@ -1528,6 +1536,10 @@ static void __devinit winbond_check2(int io, int key)
         oldid = inb(io + 2);
         outb(0xaa, io);    /* Magic Seal */
 
+        outb(origval[0], io); /* in case we poked some entirely different hardware */
+        outb(origval[1], io + 1);
+        outb(origval[2], io + 2);
+
         if (x_devid == devid && x_devrev == devrev && x_oldid == oldid)
                 goto out; /* protection against false positives */
 
@@ -1538,11 +1550,13 @@ out:
 
 static void __devinit smsc_check(int io, int key)
 {
-        int id, rev, oldid, oldrev, x_id, x_rev, x_oldid, x_oldrev;
+        int origval, id, rev, oldid, oldrev, x_id, x_rev, x_oldid, x_oldrev;
 
         if (!request_region(io, 3, __func__))
                 return;
 
+        origval = inb(io); /* Save original value */
+
         /* First probe without the key */
         outb(0x0d, io);
         x_oldid = inb(io + 1);
@@ -1566,6 +1580,8 @@ static void __devinit smsc_check(int io, int key)
         rev = inb(io + 1);
         outb(0xaa, io);    /* Magic Seal */
 
+        outb(origval, io); /* in case we poked some entirely different hardware */
+
         if (x_id == id && x_oldrev == oldrev &&
             x_oldid == oldid && x_rev == rev)
                 goto out; /* protection against false positives */
@@ -1602,11 +1618,12 @@ static void __devinit detect_and_report_smsc(void)
 static void __devinit detect_and_report_it87(void)
 {
         u16 dev;
-        u8 r;
+        u8 origval, r;
         if (verbose_probing)
                 printk(KERN_DEBUG "IT8705 Super-IO detection, now testing port 2E ...\n");
-        if (!request_region(0x2e, 1, __func__))
+        if (!request_region(0x2e, 2, __func__))
                 return;
+        origval = inb(0x2e);            /* Save original value */
         outb(0x87, 0x2e);
         outb(0x01, 0x2e);
         outb(0x55, 0x2e);
@@ -1626,8 +1643,10 @@ static void __devinit detect_and_report_it87(void)
                 outb(r | 8, 0x2F);
                 outb(0x02, 0x2E);       /* Lock */
                 outb(0x02, 0x2F);
+        } else {
+                outb(origval, 0x2e);    /* Oops, sorry to disturb */
         }
-        release_region(0x2e, 1);
+        release_region(0x2e, 2);
 }
 #endif /* CONFIG_PARPORT_PC_SUPERIO */
 
@@ -2271,6 +2290,9 @@ struct parport *parport_pc_probe_port(unsigned long int base,
                 if (IS_ERR(pdev))
                         return NULL;
                 dev = &pdev->dev;
+
+                dev->coherent_dma_mask = DMA_BIT_MASK(24);
+                dev->dma_mask = &dev->coherent_dma_mask;
         }
 
         ops = kmalloc(sizeof(struct parport_operations), GFP_KERNEL);
diff --git a/drivers/pci/Makefile b/drivers/pci/Makefile
index b77ae6794275..1ebd6b4c743b 100644
--- a/drivers/pci/Makefile
+++ b/drivers/pci/Makefile
@@ -2,10 +2,11 @@
 # Makefile for the PCI bus specific drivers.
 #
 
-obj-y           += access.o bus.o probe.o remove.o pci.o quirks.o slot.o \
+obj-y           += access.o bus.o probe.o remove.o pci.o quirks.o \
                         pci-driver.o search.o pci-sysfs.o rom.o setup-res.o \
                         irq.o
 obj-$(CONFIG_PROC_FS) += proc.o
+obj-$(CONFIG_SYSFS) += slot.o
 
 # Build PCI Express stuff if needed
 obj-$(CONFIG_PCIEPORTBUS) += pcie/
diff --git a/drivers/pci/access.c b/drivers/pci/access.c
index 0f3706512686..db23200c4874 100644
--- a/drivers/pci/access.c
+++ b/drivers/pci/access.c
@@ -66,6 +66,25 @@ EXPORT_SYMBOL(pci_bus_write_config_byte);
 EXPORT_SYMBOL(pci_bus_write_config_word);
 EXPORT_SYMBOL(pci_bus_write_config_dword);
 
+/**
+ * pci_bus_set_ops - Set raw operations of pci bus
+ * @bus:        pci bus struct
+ * @ops:        new raw operations
+ *
+ * Return previous raw operations
+ */
+struct pci_ops *pci_bus_set_ops(struct pci_bus *bus, struct pci_ops *ops)
+{
+        struct pci_ops *old_ops;
+        unsigned long flags;
+
+        spin_lock_irqsave(&pci_lock, flags);
+        old_ops = bus->ops;
+        bus->ops = ops;
+        spin_unlock_irqrestore(&pci_lock, flags);
+        return old_ops;
+}
+EXPORT_SYMBOL(pci_bus_set_ops);
 
 /**
  * pci_read_vpd - Read one entry from Vital Product Data
diff --git a/drivers/pci/bus.c b/drivers/pci/bus.c
index 97a8194063b5..cef28a79103f 100644
--- a/drivers/pci/bus.c
+++ b/drivers/pci/bus.c
@@ -41,9 +41,14 @@ pci_bus_alloc_resource(struct pci_bus *bus, struct resource *res,
                 void *alignf_data)
 {
         int i, ret = -ENOMEM;
+        resource_size_t max = -1;
 
         type_mask |= IORESOURCE_IO | IORESOURCE_MEM;
 
+        /* don't allocate too high if the pref mem doesn't support 64bit*/
+        if (!(res->flags & IORESOURCE_MEM_64))
+                max = PCIBIOS_MAX_MEM_32;
+
         for (i = 0; i < PCI_BUS_NUM_RESOURCES; i++) {
                 struct resource *r = bus->resource[i];
                 if (!r)
@@ -62,7 +67,7 @@ pci_bus_alloc_resource(struct pci_bus *bus, struct resource *res,
                 /* Ok, try it out.. */
                 ret = allocate_resource(r, res, size,
                                         r->start ? : min,
-                                        -1, align,
+                                        max, align,
                                         alignf, alignf_data);
                 if (ret == 0)
                         break;
@@ -201,13 +206,18 @@ void pci_enable_bridges(struct pci_bus *bus)
  *  Walk the given bus, including any bridged devices
  *  on buses under this bus.  Call the provided callback
  *  on each device found.
+ *
+ *  We check the return of @cb each time. If it returns anything
+ *  other than 0, we break out.
+ *
  */
-void pci_walk_bus(struct pci_bus *top, void (*cb)(struct pci_dev *, void *),
+void pci_walk_bus(struct pci_bus *top, int (*cb)(struct pci_dev *, void *),
                   void *userdata)
 {
         struct pci_dev *dev;
         struct pci_bus *bus;
         struct list_head *next;
+        int retval;
 
         bus = top;
         down_read(&pci_bus_sem);
@@ -231,8 +241,10 @@ void pci_walk_bus(struct pci_bus *top, void (*cb)(struct pci_dev *, void *),
 
                 /* Run device routines with the device locked */
                 down(&dev->dev.sem);
-                cb(dev, userdata);
+                retval = cb(dev, userdata);
                 up(&dev->dev.sem);
+                if (retval)
+                        break;
         }
         up_read(&pci_bus_sem);
 }
diff --git a/drivers/pci/dmar.c b/drivers/pci/dmar.c
index fa3a11365ec3..7b287cb38b7a 100644
--- a/drivers/pci/dmar.c
+++ b/drivers/pci/dmar.c
@@ -267,6 +267,84 @@ rmrr_parse_dev(struct dmar_rmrr_unit *rmrru)
         }
         return ret;
 }
+
+static LIST_HEAD(dmar_atsr_units);
+
+static int __init dmar_parse_one_atsr(struct acpi_dmar_header *hdr)
+{
+        struct acpi_dmar_atsr *atsr;
+        struct dmar_atsr_unit *atsru;
+
+        atsr = container_of(hdr, struct acpi_dmar_atsr, header);
+        atsru = kzalloc(sizeof(*atsru), GFP_KERNEL);
+        if (!atsru)
+                return -ENOMEM;
+
+        atsru->hdr = hdr;
+        atsru->include_all = atsr->flags & 0x1;
+
+        list_add(&atsru->list, &dmar_atsr_units);
+
+        return 0;
+}
+
+static int __init atsr_parse_dev(struct dmar_atsr_unit *atsru)
+{
+        int rc;
+        struct acpi_dmar_atsr *atsr;
+
+        if (atsru->include_all)
+                return 0;
+
+        atsr = container_of(atsru->hdr, struct acpi_dmar_atsr, header);
+        rc = dmar_parse_dev_scope((void *)(atsr + 1),
+                                (void *)atsr + atsr->header.length,
+                                &atsru->devices_cnt, &atsru->devices,
+                                atsr->segment);
+        if (rc || !atsru->devices_cnt) {
+                list_del(&atsru->list);
+                kfree(atsru);
+        }
+
+        return rc;
+}
+
+int dmar_find_matched_atsr_unit(struct pci_dev *dev)
+{
+        int i;
+        struct pci_bus *bus;
+        struct acpi_dmar_atsr *atsr;
+        struct dmar_atsr_unit *atsru;
+
+        list_for_each_entry(atsru, &dmar_atsr_units, list) {
+                atsr = container_of(atsru->hdr, struct acpi_dmar_atsr, header);
+                if (atsr->segment == pci_domain_nr(dev->bus))
+                        goto found;
+        }
+
+        return 0;
+
+found:
+        for (bus = dev->bus; bus; bus = bus->parent) {
+                struct pci_dev *bridge = bus->self;
+
+                if (!bridge || !bridge->is_pcie ||
+                    bridge->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE)
+                        return 0;
+
+                if (bridge->pcie_type == PCI_EXP_TYPE_ROOT_PORT) {
+                        for (i = 0; i < atsru->devices_cnt; i++)
+                                if (atsru->devices[i] == bridge)
+                                        return 1;
+                        break;
+                }
+        }
+
+        if (atsru->include_all)
+                return 1;
+
+        return 0;
+}
 #endif
 
 static void __init
@@ -274,22 +352,28 @@ dmar_table_print_dmar_entry(struct acpi_dmar_header *header)
 {
         struct acpi_dmar_hardware_unit *drhd;
         struct acpi_dmar_reserved_memory *rmrr;
+        struct acpi_dmar_atsr *atsr;
 
         switch (header->type) {
         case ACPI_DMAR_TYPE_HARDWARE_UNIT:
-                drhd = (struct acpi_dmar_hardware_unit *)header;
+                drhd = container_of(header, struct acpi_dmar_hardware_unit,
+                                    header);
                 printk (KERN_INFO PREFIX
-                        "DRHD (flags: 0x%08x)base: 0x%016Lx\n",
-                        drhd->flags, (unsigned long long)drhd->address);
+                        "DRHD base: %#016Lx flags: %#x\n",
+                        (unsigned long long)drhd->address, drhd->flags);
                 break;
         case ACPI_DMAR_TYPE_RESERVED_MEMORY:
-                rmrr = (struct acpi_dmar_reserved_memory *)header;
-
+                rmrr = container_of(header, struct acpi_dmar_reserved_memory,
+                                    header);
                 printk (KERN_INFO PREFIX
-                        "RMRR base: 0x%016Lx end: 0x%016Lx\n",
+                        "RMRR base: %#016Lx end: %#016Lx\n",
                         (unsigned long long)rmrr->base_address,
                         (unsigned long long)rmrr->end_address);
                 break;
+        case ACPI_DMAR_TYPE_ATSR:
+                atsr = container_of(header, struct acpi_dmar_atsr, header);
+                printk(KERN_INFO PREFIX "ATSR flags: %#x\n", atsr->flags);
+                break;
         }
 }
 
@@ -363,6 +447,11 @@ parse_dmar_table(void)
                         ret = dmar_parse_one_rmrr(entry_header);
 #endif
                         break;
+                case ACPI_DMAR_TYPE_ATSR:
+#ifdef CONFIG_DMAR
+                        ret = dmar_parse_one_atsr(entry_header);
+#endif
+                        break;
                 default:
                         printk(KERN_WARNING PREFIX
                                 "Unknown DMAR structure type\n");
@@ -431,11 +520,19 @@ int __init dmar_dev_scope_init(void)
 #ifdef CONFIG_DMAR
         {
                 struct dmar_rmrr_unit *rmrr, *rmrr_n;
+                struct dmar_atsr_unit *atsr, *atsr_n;
+
                 list_for_each_entry_safe(rmrr, rmrr_n, &dmar_rmrr_units, list) {
                         ret = rmrr_parse_dev(rmrr);
                         if (ret)
                                 return ret;
                 }
+
+                list_for_each_entry_safe(atsr, atsr_n, &dmar_atsr_units, list) {
+                        ret = atsr_parse_dev(atsr);
+                        if (ret)
+                                return ret;
+                }
         }
 #endif
 
@@ -468,6 +565,9 @@ int __init dmar_table_init(void)
 #ifdef CONFIG_DMAR
         if (list_empty(&dmar_rmrr_units))
                 printk(KERN_INFO PREFIX "No RMRR found\n");
+
+        if (list_empty(&dmar_atsr_units))
+                printk(KERN_INFO PREFIX "No ATSR found\n");
 #endif
 
 #ifdef CONFIG_INTR_REMAP
@@ -515,6 +615,7 @@ int alloc_iommu(struct dmar_drhd_unit *drhd)
         u32 ver;
         static int iommu_allocated = 0;
         int agaw = 0;
+        int msagaw = 0;
 
         iommu = kzalloc(sizeof(*iommu), GFP_KERNEL);
         if (!iommu)
@@ -535,12 +636,20 @@ int alloc_iommu(struct dmar_drhd_unit *drhd)
         agaw = iommu_calculate_agaw(iommu);
         if (agaw < 0) {
                 printk(KERN_ERR
-                        "Cannot get a valid agaw for iommu (seq_id = %d)\n",
+                       "Cannot get a valid agaw for iommu (seq_id = %d)\n",
+                       iommu->seq_id);
+                goto error;
+        }
+        msagaw = iommu_calculate_max_sagaw(iommu);
+        if (msagaw < 0) {
+                printk(KERN_ERR
+                        "Cannot get a valid max agaw for iommu (seq_id = %d)\n",
                         iommu->seq_id);
                 goto error;
         }
 #endif
         iommu->agaw = agaw;
+        iommu->msagaw = msagaw;
 
         /* the registers might be more than one page */
         map_size = max_t(int, ecap_max_iotlb_offset(iommu->ecap),
@@ -590,7 +699,8 @@ void free_iommu(struct intel_iommu *iommu)
  */
 static inline void reclaim_free_desc(struct q_inval *qi)
 {
-        while (qi->desc_status[qi->free_tail] == QI_DONE) {
+        while (qi->desc_status[qi->free_tail] == QI_DONE ||
+               qi->desc_status[qi->free_tail] == QI_ABORT) {
                 qi->desc_status[qi->free_tail] = QI_FREE;
                 qi->free_tail = (qi->free_tail + 1) % QI_LENGTH;
                 qi->free_cnt++;
@@ -600,10 +710,13 @@ static inline void reclaim_free_desc(struct q_inval *qi)
 static int qi_check_fault(struct intel_iommu *iommu, int index)
 {
         u32 fault;
-        int head;
+        int head, tail;
         struct q_inval *qi = iommu->qi;
         int wait_index = (index + 1) % QI_LENGTH;
 
+        if (qi->desc_status[wait_index] == QI_ABORT)
+                return -EAGAIN;
+
         fault = readl(iommu->reg + DMAR_FSTS_REG);
 
         /*
@@ -613,7 +726,11 @@ static int qi_check_fault(struct intel_iommu *iommu, int index)
          */
         if (fault & DMA_FSTS_IQE) {
                 head = readl(iommu->reg + DMAR_IQH_REG);
-                if ((head >> 4) == index) {
+                if ((head >> DMAR_IQ_SHIFT) == index) {
+                        printk(KERN_ERR "VT-d detected invalid descriptor: "
+                                "low=%llx, high=%llx\n",
+                                (unsigned long long)qi->desc[index].low,
+                                (unsigned long long)qi->desc[index].high);
                         memcpy(&qi->desc[index], &qi->desc[wait_index],
                                         sizeof(struct qi_desc));
                         __iommu_flush_cache(iommu, &qi->desc[index],
@@ -623,6 +740,32 @@ static int qi_check_fault(struct intel_iommu *iommu, int index)
                 }
         }
 
+        /*
+         * If ITE happens, all pending wait_desc commands are aborted.
+         * No new descriptors are fetched until the ITE is cleared.
+         */
+        if (fault & DMA_FSTS_ITE) {
+                head = readl(iommu->reg + DMAR_IQH_REG);
+                head = ((head >> DMAR_IQ_SHIFT) - 1 + QI_LENGTH) % QI_LENGTH;
+                head |= 1;
+                tail = readl(iommu->reg + DMAR_IQT_REG);
+                tail = ((tail >> DMAR_IQ_SHIFT) - 1 + QI_LENGTH) % QI_LENGTH;
+
+                writel(DMA_FSTS_ITE, iommu->reg + DMAR_FSTS_REG);
+
+                do {
+                        if (qi->desc_status[head] == QI_IN_USE)
+                                qi->desc_status[head] = QI_ABORT;
+                        head = (head - 2 + QI_LENGTH) % QI_LENGTH;
+                } while (head != tail);
+
+                if (qi->desc_status[wait_index] == QI_ABORT)
+                        return -EAGAIN;
+        }
+
+        if (fault & DMA_FSTS_ICE)
+                writel(DMA_FSTS_ICE, iommu->reg + DMAR_FSTS_REG);
+
         return 0;
 }
 
@@ -632,7 +775,7 @@ static int qi_check_fault(struct intel_iommu *iommu, int index)
  */
 int qi_submit_sync(struct qi_desc *desc, struct intel_iommu *iommu)
 {
-        int rc = 0;
+        int rc;
         struct q_inval *qi = iommu->qi;
         struct qi_desc *hw, wait_desc;
         int wait_index, index;
@@ -643,6 +786,9 @@ int qi_submit_sync(struct qi_desc *desc, struct intel_iommu *iommu)
 
         hw = qi->desc;
 
+restart:
+        rc = 0;
+
         spin_lock_irqsave(&qi->q_lock, flags);
         while (qi->free_cnt < 3) {
                 spin_unlock_irqrestore(&qi->q_lock, flags);
@@ -673,7 +819,7 @@ int qi_submit_sync(struct qi_desc *desc, struct intel_iommu *iommu)
          * update the HW tail register indicating the presence of
          * new descriptors.
          */
-        writel(qi->free_head << 4, iommu->reg + DMAR_IQT_REG);
+        writel(qi->free_head << DMAR_IQ_SHIFT, iommu->reg + DMAR_IQT_REG);
 
         while (qi->desc_status[wait_index] != QI_DONE) {
                 /*
@@ -685,18 +831,21 @@ int qi_submit_sync(struct qi_desc *desc, struct intel_iommu *iommu)
                  */
                 rc = qi_check_fault(iommu, index);
                 if (rc)
-                        goto out;
+                        break;
 
                 spin_unlock(&qi->q_lock);
                 cpu_relax();
                 spin_lock(&qi->q_lock);
         }
-out:
-        qi->desc_status[index] = qi->desc_status[wait_index] = QI_DONE;
+
+        qi->desc_status[index] = QI_DONE;
 
         reclaim_free_desc(qi);
         spin_unlock_irqrestore(&qi->q_lock, flags);
 
+        if (rc == -EAGAIN)
+                goto restart;
+
         return rc;
 }
 
@@ -714,41 +863,26 @@ void qi_global_iec(struct intel_iommu *iommu)
         qi_submit_sync(&desc, iommu);
 }
 
-int qi_flush_context(struct intel_iommu *iommu, u16 did, u16 sid, u8 fm,
-                     u64 type, int non_present_entry_flush)
+void qi_flush_context(struct intel_iommu *iommu, u16 did, u16 sid, u8 fm,
+                      u64 type)
 {
         struct qi_desc desc;
 
-        if (non_present_entry_flush) {
-                if (!cap_caching_mode(iommu->cap))
-                        return 1;
-                else
-                        did = 0;
-        }
-
         desc.low = QI_CC_FM(fm) | QI_CC_SID(sid) | QI_CC_DID(did)
                         | QI_CC_GRAN(type) | QI_CC_TYPE;
         desc.high = 0;
 
-        return qi_submit_sync(&desc, iommu);
+        qi_submit_sync(&desc, iommu);
 }
 
-int qi_flush_iotlb(struct intel_iommu *iommu, u16 did, u64 addr,
-                   unsigned int size_order, u64 type,
-                   int non_present_entry_flush)
+void qi_flush_iotlb(struct intel_iommu *iommu, u16 did, u64 addr,
+                    unsigned int size_order, u64 type)
 {
         u8 dw = 0, dr = 0;
 
         struct qi_desc desc;
         int ih = 0;
 
-        if (non_present_entry_flush) {
-                if (!cap_caching_mode(iommu->cap))
-                        return 1;
-                else
-                        did = 0;
-        }
-
         if (cap_write_drain(iommu->cap))
                 dw = 1;
 
@@ -760,7 +894,28 @@ int qi_flush_iotlb(struct intel_iommu *iommu, u16 did, u64 addr,
         desc.high = QI_IOTLB_ADDR(addr) | QI_IOTLB_IH(ih)
                 | QI_IOTLB_AM(size_order);
 
-        return qi_submit_sync(&desc, iommu);
+        qi_submit_sync(&desc, iommu);
+}
+
+void qi_flush_dev_iotlb(struct intel_iommu *iommu, u16 sid, u16 qdep,
+                        u64 addr, unsigned mask)
+{
+        struct qi_desc desc;
+
+        if (mask) {
+                BUG_ON(addr & ((1 << (VTD_PAGE_SHIFT + mask)) - 1));
+                addr |= (1 << (VTD_PAGE_SHIFT + mask - 1)) - 1;
+                desc.high = QI_DEV_IOTLB_ADDR(addr) | QI_DEV_IOTLB_SIZE;
+        } else
+                desc.high = QI_DEV_IOTLB_ADDR(addr);
+
+        if (qdep >= QI_DEV_IOTLB_MAX_INVS)
+                qdep = 0;
+
+        desc.low = QI_DEV_IOTLB_SID(sid) | QI_DEV_IOTLB_QDEP(qdep) |
+                   QI_DIOTLB_TYPE;
+
+        qi_submit_sync(&desc, iommu);
 }
 
 /*
@@ -790,7 +945,6 @@ void dmar_disable_qi(struct intel_iommu *iommu)
                 cpu_relax();
 
         iommu->gcmd &= ~DMA_GCMD_QIE;
-
         writel(iommu->gcmd, iommu->reg + DMAR_GCMD_REG);
 
         IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG, readl,
@@ -804,7 +958,7 @@ end:
  */
 static void __dmar_enable_qi(struct intel_iommu *iommu)
 {
-        u32 cmd, sts;
+        u32 sts;
         unsigned long flags;
         struct q_inval *qi = iommu->qi;
 
@@ -818,9 +972,8 @@ static void __dmar_enable_qi(struct intel_iommu *iommu)
 
         dmar_writeq(iommu->reg + DMAR_IQA_REG, virt_to_phys(qi->desc));
 
-        cmd = iommu->gcmd | DMA_GCMD_QIE;
         iommu->gcmd |= DMA_GCMD_QIE;
-        writel(cmd, iommu->reg + DMAR_GCMD_REG);
+        writel(iommu->gcmd, iommu->reg + DMAR_GCMD_REG);
 
         /* Make sure hardware complete it */
         IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG, readl, (sts & DMA_GSTS_QIES), sts);
@@ -1096,7 +1249,7 @@ int dmar_set_interrupt(struct intel_iommu *iommu)
                 set_irq_data(irq, NULL);
                 iommu->irq = 0;
                 destroy_irq(irq);
-                return 0;
+                return ret;
         }
 
         ret = request_irq(irq, dmar_fault, 0, iommu->name, iommu);
diff --git a/drivers/pci/hotplug/Kconfig b/drivers/pci/hotplug/Kconfig
index 9aa4fe100a0d..66f29bc00be4 100644
--- a/drivers/pci/hotplug/Kconfig
+++ b/drivers/pci/hotplug/Kconfig
@@ -4,7 +4,7 @@
 
 menuconfig HOTPLUG_PCI
         tristate "Support for PCI Hotplug"
-        depends on PCI && HOTPLUG
+        depends on PCI && HOTPLUG && SYSFS
         ---help---
           Say Y here if you have a motherboard with a PCI Hotplug controller.
           This allows you to add and remove PCI cards while the machine is
@@ -41,7 +41,7 @@ config HOTPLUG_PCI_FAKE
 
 config HOTPLUG_PCI_COMPAQ
         tristate "Compaq PCI Hotplug driver"
-        depends on X86 && PCI_BIOS && PCI_LEGACY
+        depends on X86 && PCI_BIOS
         help
           Say Y here if you have a motherboard with a Compaq PCI Hotplug
           controller.
diff --git a/drivers/pci/hotplug/acpiphp_core.c b/drivers/pci/hotplug/acpiphp_core.c
index 43c10bd261b4..4dd7114964ac 100644
--- a/drivers/pci/hotplug/acpiphp_core.c
+++ b/drivers/pci/hotplug/acpiphp_core.c
@@ -77,7 +77,6 @@ static int get_latch_status	(struct hotplug_slot *slot, u8 *value);
 static int get_adapter_status   (struct hotplug_slot *slot, u8 *value);
 
 static struct hotplug_slot_ops acpi_hotplug_slot_ops = {
-        .owner                  = THIS_MODULE,
         .enable_slot            = enable_slot,
         .disable_slot           = disable_slot,
         .set_attention_status   = set_attention_status,
diff --git a/drivers/pci/hotplug/cpci_hotplug_core.c b/drivers/pci/hotplug/cpci_hotplug_core.c
index de94f4feef8c..a5b9f6ae507b 100644
--- a/drivers/pci/hotplug/cpci_hotplug_core.c
+++ b/drivers/pci/hotplug/cpci_hotplug_core.c
@@ -72,7 +72,6 @@ static int get_adapter_status(struct hotplug_slot *slot, u8 * value);
 static int get_latch_status(struct hotplug_slot *slot, u8 * value);
 
 static struct hotplug_slot_ops cpci_hotplug_slot_ops = {
-        .owner = THIS_MODULE,
         .enable_slot = enable_slot,
         .disable_slot = disable_slot,
         .set_attention_status = set_attention_status,
diff --git a/drivers/pci/hotplug/cpqphp.h b/drivers/pci/hotplug/cpqphp.h
index afaf8f69f73e..53836001d511 100644
--- a/drivers/pci/hotplug/cpqphp.h
+++ b/drivers/pci/hotplug/cpqphp.h
@@ -150,25 +150,25 @@ struct ctrl_reg {			/* offset */
 
 /* offsets to the controller registers based on the above structure layout */
 enum ctrl_offsets {
-        SLOT_RST =              offsetof(struct ctrl_reg, slot_RST),
+        SLOT_RST =              offsetof(struct ctrl_reg, slot_RST),
         SLOT_ENABLE =           offsetof(struct ctrl_reg, slot_enable),
         MISC =                  offsetof(struct ctrl_reg, misc),
         LED_CONTROL =           offsetof(struct ctrl_reg, led_control),
         INT_INPUT_CLEAR =       offsetof(struct ctrl_reg, int_input_clear),
-        INT_MASK =              offsetof(struct ctrl_reg, int_mask),
-        CTRL_RESERVED0 =        offsetof(struct ctrl_reg, reserved0),
+        INT_MASK =              offsetof(struct ctrl_reg, int_mask),
+        CTRL_RESERVED0 =        offsetof(struct ctrl_reg, reserved0),
         CTRL_RESERVED1 =        offsetof(struct ctrl_reg, reserved1),
         CTRL_RESERVED2 =        offsetof(struct ctrl_reg, reserved1),
-        GEN_OUTPUT_AB =         offsetof(struct ctrl_reg, gen_output_AB),
-        NON_INT_INPUT =         offsetof(struct ctrl_reg, non_int_input),
+        GEN_OUTPUT_AB =         offsetof(struct ctrl_reg, gen_output_AB),
+        NON_INT_INPUT =         offsetof(struct ctrl_reg, non_int_input),
         CTRL_RESERVED3 =        offsetof(struct ctrl_reg, reserved3),
         CTRL_RESERVED4 =        offsetof(struct ctrl_reg, reserved4),
         CTRL_RESERVED5 =        offsetof(struct ctrl_reg, reserved5),
         CTRL_RESERVED6 =        offsetof(struct ctrl_reg, reserved6),
         CTRL_RESERVED7 =        offsetof(struct ctrl_reg, reserved7),
         CTRL_RESERVED8 =        offsetof(struct ctrl_reg, reserved8),
-        SLOT_MASK =             offsetof(struct ctrl_reg, slot_mask),
-        CTRL_RESERVED9 =        offsetof(struct ctrl_reg, reserved9),
+        SLOT_MASK =             offsetof(struct ctrl_reg, slot_mask),
+        CTRL_RESERVED9 =        offsetof(struct ctrl_reg, reserved9),
         CTRL_RESERVED10 =       offsetof(struct ctrl_reg, reserved10),
         CTRL_RESERVED11 =       offsetof(struct ctrl_reg, reserved11),
         SLOT_SERR =             offsetof(struct ctrl_reg, slot_SERR),
@@ -190,7 +190,9 @@ struct hrt {
         u32 reserved2;
 } __attribute__ ((packed));
 
-/* offsets to the hotplug resource table registers based on the above structure layout */
+/* offsets to the hotplug resource table registers based on the above
+ * structure layout
+ */
 enum hrt_offsets {
         SIG0 =                  offsetof(struct hrt, sig0),
         SIG1 =                  offsetof(struct hrt, sig1),
@@ -217,18 +219,20 @@ struct slot_rt {
         u16 pre_mem_length;
 } __attribute__ ((packed));
 
-/* offsets to the hotplug slot resource table registers based on the above structure layout */
+/* offsets to the hotplug slot resource table registers based on the above
+ * structure layout
+ */
 enum slot_rt_offsets {
         DEV_FUNC =              offsetof(struct slot_rt, dev_func),
-        PRIMARY_BUS =           offsetof(struct slot_rt, primary_bus),
-        SECONDARY_BUS =         offsetof(struct slot_rt, secondary_bus),
-        MAX_BUS =               offsetof(struct slot_rt, max_bus),
-        IO_BASE =               offsetof(struct slot_rt, io_base),
-        IO_LENGTH =             offsetof(struct slot_rt, io_length),
-        MEM_BASE =              offsetof(struct slot_rt, mem_base),
-        MEM_LENGTH =            offsetof(struct slot_rt, mem_length),
-        PRE_MEM_BASE =          offsetof(struct slot_rt, pre_mem_base),
-        PRE_MEM_LENGTH =        offsetof(struct slot_rt, pre_mem_length),
+        PRIMARY_BUS =           offsetof(struct slot_rt, primary_bus),
+        SECONDARY_BUS =         offsetof(struct slot_rt, secondary_bus),
+        MAX_BUS =               offsetof(struct slot_rt, max_bus),
+        IO_BASE =               offsetof(struct slot_rt, io_base),
+        IO_LENGTH =             offsetof(struct slot_rt, io_length),
+        MEM_BASE =              offsetof(struct slot_rt, mem_base),
+        MEM_LENGTH =            offsetof(struct slot_rt, mem_length),
+        PRE_MEM_BASE =          offsetof(struct slot_rt, pre_mem_base),
+        PRE_MEM_LENGTH =        offsetof(struct slot_rt, pre_mem_length),
 };
 
 struct pci_func {
@@ -286,8 +290,8 @@ struct event_info {
 struct controller {
         struct controller *next;
         u32 ctrl_int_comp;
-        struct mutex crit_sect;         /* critical section mutex */
-        void __iomem *hpc_reg;          /* cookie for our pci controller location */
+        struct mutex crit_sect; /* critical section mutex */
+        void __iomem *hpc_reg;  /* cookie for our pci controller location */
         struct pci_resource *mem_head;
         struct pci_resource *p_mem_head;
         struct pci_resource *io_head;
@@ -299,7 +303,7 @@ struct controller {
         u8 next_event;
         u8 interrupt;
         u8 cfgspc_irq;
-        u8 bus;                         /* bus number for the pci hotplug controller */
+        u8 bus;                 /* bus number for the pci hotplug controller */
         u8 rev;
         u8 slot_device_offset;
         u8 first_slot;
@@ -401,46 +405,57 @@ struct resource_lists {
 
 
 /* debugfs functions for the hotplug controller info */
-extern void cpqhp_initialize_debugfs            (void);
-extern void cpqhp_shutdown_debugfs              (void);
-extern void cpqhp_create_debugfs_files          (struct controller *ctrl);
-extern void cpqhp_remove_debugfs_files          (struct controller *ctrl);
+extern void cpqhp_initialize_debugfs(void);
+extern void cpqhp_shutdown_debugfs(void);
+extern void cpqhp_create_debugfs_files(struct controller *ctrl);
+extern void cpqhp_remove_debugfs_files(struct controller *ctrl);
 
 /* controller functions */
-extern void     cpqhp_pushbutton_thread         (unsigned long event_pointer);
-extern irqreturn_t cpqhp_ctrl_intr              (int IRQ, void *data);
-extern int      cpqhp_find_available_resources  (struct controller *ctrl, void __iomem *rom_start);
-extern int      cpqhp_event_start_thread        (void);
-extern void     cpqhp_event_stop_thread         (void);
-extern struct pci_func *cpqhp_slot_create       (unsigned char busnumber);
-extern struct pci_func *cpqhp_slot_find         (unsigned char bus, unsigned char device, unsigned char index);
-extern int      cpqhp_process_SI                (struct controller *ctrl, struct pci_func *func);
-extern int      cpqhp_process_SS                (struct controller *ctrl, struct pci_func *func);
-extern int      cpqhp_hardware_test             (struct controller *ctrl, int test_num);
+extern void cpqhp_pushbutton_thread(unsigned long event_pointer);
+extern irqreturn_t cpqhp_ctrl_intr(int IRQ, void *data);
+extern int cpqhp_find_available_resources(struct controller *ctrl,
+                                          void __iomem *rom_start);
+extern int cpqhp_event_start_thread(void);
+extern void cpqhp_event_stop_thread(void);
+extern struct pci_func *cpqhp_slot_create(unsigned char busnumber);
+extern struct pci_func *cpqhp_slot_find(unsigned char bus, unsigned char device,
+                                        unsigned char index);
+extern int cpqhp_process_SI(struct controller *ctrl, struct pci_func *func);
+extern int cpqhp_process_SS(struct controller *ctrl, struct pci_func *func);
+extern int cpqhp_hardware_test(struct controller *ctrl, int test_num);
 
 /* resource functions */
 extern int      cpqhp_resource_sort_and_combine (struct pci_resource **head);
 
 /* pci functions */
-extern int      cpqhp_set_irq                   (u8 bus_num, u8 dev_num, u8 int_pin, u8 irq_num);
-extern int      cpqhp_get_bus_dev               (struct controller *ctrl, u8 *bus_num, u8 *dev_num, u8 slot);
-extern int      cpqhp_save_config               (struct controller *ctrl, int busnumber, int is_hot_plug);
-extern int      cpqhp_save_base_addr_length     (struct controller *ctrl, struct pci_func * func);
-extern int      cpqhp_save_used_resources       (struct controller *ctrl, struct pci_func * func);
-extern int      cpqhp_configure_board           (struct controller *ctrl, struct pci_func * func);
-extern int      cpqhp_save_slot_config          (struct controller *ctrl, struct pci_func * new_slot);
-extern int      cpqhp_valid_replace             (struct controller *ctrl, struct pci_func * func);
-extern void     cpqhp_destroy_board_resources   (struct pci_func * func);
-extern int      cpqhp_return_board_resources    (struct pci_func * func, struct resource_lists * resources);
-extern void     cpqhp_destroy_resource_list     (struct resource_lists * resources);
-extern int      cpqhp_configure_device          (struct controller* ctrl, struct pci_func* func);
-extern int      cpqhp_unconfigure_device        (struct pci_func* func);
+extern int cpqhp_set_irq(u8 bus_num, u8 dev_num, u8 int_pin, u8 irq_num);
+extern int cpqhp_get_bus_dev(struct controller *ctrl, u8 *bus_num, u8 *dev_num,
+                             u8 slot);
+extern int cpqhp_save_config(struct controller *ctrl, int busnumber,
+                             int is_hot_plug);
+extern int cpqhp_save_base_addr_length(struct controller *ctrl,
+                                       struct pci_func *func);
+extern int cpqhp_save_used_resources(struct controller *ctrl,
+                                     struct pci_func *func);
+extern int cpqhp_configure_board(struct controller *ctrl,
+                                 struct pci_func *func);
+extern int cpqhp_save_slot_config(struct controller *ctrl,
+                                  struct pci_func *new_slot);
+extern int cpqhp_valid_replace(struct controller *ctrl, struct pci_func *func);
+extern void cpqhp_destroy_board_resources(struct pci_func *func);
+extern int cpqhp_return_board_resources (struct pci_func *func,
+                                         struct resource_lists *resources);
+extern void cpqhp_destroy_resource_list(struct resource_lists *resources);
+extern int cpqhp_configure_device(struct controller *ctrl,
+                                  struct pci_func *func);
+extern int cpqhp_unconfigure_device(struct pci_func *func);
 
 /* Global variables */
 extern int cpqhp_debug;
 extern int cpqhp_legacy_mode;
 extern struct controller *cpqhp_ctrl_list;
 extern struct pci_func *cpqhp_slot_list[256];
+extern struct irq_routing_table *cpqhp_routing_table;
 
 /* these can be gotten rid of, but for debugging they are purty */
 extern u8 cpqhp_nic_irq;
@@ -449,7 +464,7 @@ extern u8 cpqhp_disk_irq;
 
 /* inline functions */
 
-static inline char *slot_name(struct slot *slot)
+static inline const char *slot_name(struct slot *slot)
 {
         return hotplug_slot_name(slot->hotplug_slot);
 }
@@ -458,9 +473,9 @@ static inline char *slot_name(struct slot *slot)
  * return_resource
  *
  * Puts node back in the resource list pointed to by head
- *
  */
-static inline void return_resource(struct pci_resource **head, struct pci_resource *node)
+static inline void return_resource(struct pci_resource **head,
+                                   struct pci_resource *node)
 {
         if (!node || !head)
                 return;
@@ -471,7 +486,7 @@ static inline void return_resource(struct pci_resource **head, struct pci_resour
 static inline void set_SOGO(struct controller *ctrl)
 {
         u16 misc;
-        
+
         misc = readw(ctrl->hpc_reg + MISC);
         misc = (misc | 0x0001) & 0xFFFB;
         writew(misc, ctrl->hpc_reg + MISC);
@@ -481,7 +496,7 @@ static inline void set_SOGO(struct controller *ctrl)
 static inline void amber_LED_on(struct controller *ctrl, u8 slot)
 {
         u32 led_control;
-        
+
         led_control = readl(ctrl->hpc_reg + LED_CONTROL);
         led_control |= (0x01010000L << slot);
         writel(led_control, ctrl->hpc_reg + LED_CONTROL);
@@ -491,7 +506,7 @@ static inline void amber_LED_on(struct controller *ctrl, u8 slot)
 static inline void amber_LED_off(struct controller *ctrl, u8 slot)
 {
         u32 led_control;
-        
+
         led_control = readl(ctrl->hpc_reg + LED_CONTROL);
         led_control &= ~(0x01010000L << slot);
         writel(led_control, ctrl->hpc_reg + LED_CONTROL);
@@ -504,7 +519,7 @@ static inline int read_amber_LED(struct controller *ctrl, u8 slot)
 
         led_control = readl(ctrl->hpc_reg + LED_CONTROL);
         led_control &= (0x01010000L << slot);
-        
+
         return led_control ? 1 : 0;
 }
 
@@ -512,7 +527,7 @@ static inline int read_amber_LED(struct controller *ctrl, u8 slot)
 static inline void green_LED_on(struct controller *ctrl, u8 slot)
 {
         u32 led_control;
-        
+
         led_control = readl(ctrl->hpc_reg + LED_CONTROL);
         led_control |= 0x0101L << slot;
         writel(led_control, ctrl->hpc_reg + LED_CONTROL);
@@ -521,7 +536,7 @@ static inline void green_LED_on(struct controller *ctrl, u8 slot)
 static inline void green_LED_off(struct controller *ctrl, u8 slot)
 {
         u32 led_control;
-        
+
         led_control = readl(ctrl->hpc_reg + LED_CONTROL);
         led_control &= ~(0x0101L << slot);
         writel(led_control, ctrl->hpc_reg + LED_CONTROL);
@@ -531,7 +546,7 @@ static inline void green_LED_off(struct controller *ctrl, u8 slot)
 static inline void green_LED_blink(struct controller *ctrl, u8 slot)
 {
         u32 led_control;
-        
+
         led_control = readl(ctrl->hpc_reg + LED_CONTROL);
         led_control &= ~(0x0101L << slot);
         led_control |= (0x0001L << slot);
@@ -575,22 +590,21 @@ static inline u8 read_slot_enable(struct controller *ctrl)
 }
 
 
-/*
+/**
  * get_controller_speed - find the current frequency/mode of controller.
  *
  * @ctrl: controller to get frequency/mode for.
  *
  * Returns controller speed.
- *
  */
 static inline u8 get_controller_speed(struct controller *ctrl)
 {
         u8 curr_freq;
-        u16 misc;
-        
+        u16 misc;
+
         if (ctrl->pcix_support) {
                 curr_freq = readb(ctrl->hpc_reg + NEXT_CURR_FREQ);
-                if ((curr_freq & 0xB0) == 0xB0) 
+                if ((curr_freq & 0xB0) == 0xB0)
                         return PCI_SPEED_133MHz_PCIX;
                 if ((curr_freq & 0xA0) == 0xA0)
                         return PCI_SPEED_100MHz_PCIX;
@@ -602,19 +616,18 @@ static inline u8 get_controller_speed(struct controller *ctrl)
                 return PCI_SPEED_33MHz;
         }
 
-        misc = readw(ctrl->hpc_reg + MISC);
-        return (misc & 0x0800) ? PCI_SPEED_66MHz : PCI_SPEED_33MHz;
+        misc = readw(ctrl->hpc_reg + MISC);
+        return (misc & 0x0800) ? PCI_SPEED_66MHz : PCI_SPEED_33MHz;
 }
- 
 
-/*
+
+/**
  * get_adapter_speed - find the max supported frequency/mode of adapter.
  *
  * @ctrl: hotplug controller.
  * @hp_slot: hotplug slot where adapter is installed.
  *
  * Returns adapter speed.
- *
  */
 static inline u8 get_adapter_speed(struct controller *ctrl, u8 hp_slot)
 {
@@ -672,7 +685,8 @@ static inline int get_slot_enabled(struct controller *ctrl, struct slot *slot)
 }
 
 
-static inline int cpq_get_latch_status(struct controller *ctrl, struct slot *slot)
+static inline int cpq_get_latch_status(struct controller *ctrl,
+                                       struct slot *slot)
 {
         u32 status;
         u8 hp_slot;
@@ -687,7 +701,8 @@ static inline int cpq_get_latch_status(struct controller *ctrl, struct slot *slo
 }
 
 
-static inline int get_presence_status(struct controller *ctrl, struct slot *slot)
+static inline int get_presence_status(struct controller *ctrl,
+                                      struct slot *slot)
 {
         int presence_save = 0;
         u8 hp_slot;
@@ -696,7 +711,8 @@ static inline int get_presence_status(struct controller *ctrl, struct slot *slot
         hp_slot = slot->device - ctrl->slot_device_offset;
 
         tempdword = readl(ctrl->hpc_reg + INT_INPUT_CLEAR);
-        presence_save = (int) ((((~tempdword) >> 23) | ((~tempdword) >> 15)) >> hp_slot) & 0x02;
+        presence_save = (int) ((((~tempdword) >> 23) | ((~tempdword) >> 15))
+                                >> hp_slot) & 0x02;
 
         return presence_save;
 }
@@ -718,5 +734,12 @@ static inline int wait_for_ctrl_irq(struct controller *ctrl)
         return retval;
 }
 
-#endif
+#include <asm/pci_x86.h>
+static inline int cpqhp_routing_table_length(void)
+{
+        BUG_ON(cpqhp_routing_table == NULL);
+        return ((cpqhp_routing_table->size - sizeof(struct irq_routing_table)) /
+                sizeof(struct irq_info));
+}
 
+#endif
diff --git a/drivers/pci/hotplug/cpqphp_core.c b/drivers/pci/hotplug/cpqphp_core.c
index c2e1bcbb28a7..075b4f4b6e0d 100644
--- a/drivers/pci/hotplug/cpqphp_core.c
+++ b/drivers/pci/hotplug/cpqphp_core.c
@@ -25,8 +25,7 @@
  * Send feedback to <greg@kroah.com>
  *
  * Jan 12, 2003 -       Added 66/100/133MHz PCI-X support,
- *                      Torben Mathiasen <torben.mathiasen@hp.com>
- *
+ *                      Torben Mathiasen <torben.mathiasen@hp.com>
  */
 
 #include <linux/module.h>
@@ -45,7 +44,6 @@
 
 #include "cpqphp.h"
 #include "cpqphp_nvram.h"
-#include <asm/pci_x86.h>
 
 
 /* Global variables */
@@ -53,6 +51,7 @@ int cpqhp_debug;
 int cpqhp_legacy_mode;
 struct controller *cpqhp_ctrl_list;     /* = NULL */
 struct pci_func *cpqhp_slot_list[256];
+struct irq_routing_table *cpqhp_routing_table;
 
 /* local variables */
 static void __iomem *smbios_table;
@@ -78,33 +77,6 @@ MODULE_PARM_DESC(debug, "Debugging mode enabled or not");
 
 #define CPQHPC_MODULE_MINOR 208
 
-static int one_time_init        (void);
-static int set_attention_status (struct hotplug_slot *slot, u8 value);
-static int process_SI           (struct hotplug_slot *slot);
-static int process_SS           (struct hotplug_slot *slot);
-static int hardware_test        (struct hotplug_slot *slot, u32 value);
-static int get_power_status     (struct hotplug_slot *slot, u8 *value);
-static int get_attention_status (struct hotplug_slot *slot, u8 *value);
-static int get_latch_status     (struct hotplug_slot *slot, u8 *value);
-static int get_adapter_status   (struct hotplug_slot *slot, u8 *value);
-static int get_max_bus_speed    (struct hotplug_slot *slot, enum pci_bus_speed *value);
-static int get_cur_bus_speed    (struct hotplug_slot *slot, enum pci_bus_speed *value);
-
-static struct hotplug_slot_ops cpqphp_hotplug_slot_ops = {
-        .owner =                THIS_MODULE,
-        .set_attention_status = set_attention_status,
-        .enable_slot =          process_SI,
-        .disable_slot =         process_SS,
-        .hardware_test =        hardware_test,
-        .get_power_status =     get_power_status,
-        .get_attention_status = get_attention_status,
-        .get_latch_status =     get_latch_status,
-        .get_adapter_status =   get_adapter_status,
-        .get_max_bus_speed =    get_max_bus_speed,
-        .get_cur_bus_speed =    get_cur_bus_speed,
-};
-
-
 static inline int is_slot64bit(struct slot *slot)
 {
         return (readb(slot->p_sm_slot + SMBIOS_SLOT_WIDTH) == 0x06) ? 1 : 0;
@@ -144,7 +116,7 @@ static void __iomem * detect_SMBIOS_pointer(void __iomem *begin, void __iomem *e
                         break;
                 }
         }
-        
+
         if (!status)
                 fp = NULL;
 
@@ -171,7 +143,7 @@ static int init_SERR(struct controller * ctrl)
         tempdword = ctrl->first_slot;
 
         number_of_slots = readb(ctrl->hpc_reg + SLOT_MASK) & 0x0F;
-        // Loop through slots
+        /* Loop through slots */
         while (number_of_slots) {
                 physical_slot = tempdword;
                 writeb(0, ctrl->hpc_reg + SLOT_SERR);
@@ -182,41 +154,42 @@ static int init_SERR(struct controller * ctrl)
         return 0;
 }
 
-
-/* nice debugging output */
-static int pci_print_IRQ_route (void)
+static int init_cpqhp_routing_table(void)
 {
-        struct irq_routing_table *routing_table;
         int len;
-        int loop;
-
-        u8 tbus, tdevice, tslot;
 
-        routing_table = pcibios_get_irq_routing_table();
-        if (routing_table == NULL) {
-                err("No BIOS Routing Table??? Not good\n");
+        cpqhp_routing_table = pcibios_get_irq_routing_table();
+        if (cpqhp_routing_table == NULL)
                 return -ENOMEM;
-        }
 
-        len = (routing_table->size - sizeof(struct irq_routing_table)) /
-                        sizeof(struct irq_info);
-        // Make sure I got at least one entry
+        len = cpqhp_routing_table_length();
         if (len == 0) {
-                kfree(routing_table);
+                kfree(cpqhp_routing_table);
+                cpqhp_routing_table = NULL;
                 return -1;
         }
 
-        dbg("bus dev func slot\n");
+        return 0;
+}
+
+/* nice debugging output */
+static void pci_print_IRQ_route(void)
+{
+        int len;
+        int loop;
+        u8 tbus, tdevice, tslot;
+
+        len = cpqhp_routing_table_length();
 
+        dbg("bus dev func slot\n");
         for (loop = 0; loop < len; ++loop) {
-                tbus = routing_table->slots[loop].bus;
-                tdevice = routing_table->slots[loop].devfn;
-                tslot = routing_table->slots[loop].slot;
+                tbus = cpqhp_routing_table->slots[loop].bus;
+                tdevice = cpqhp_routing_table->slots[loop].devfn;
+                tslot = cpqhp_routing_table->slots[loop].slot;
                 dbg("%d %d %d %d\n", tbus, tdevice >> 3, tdevice & 0x7, tslot);
 
         }
-        kfree(routing_table);
-        return 0;
+        return;
 }
 
 
@@ -242,9 +215,9 @@ static void __iomem *get_subsequent_smbios_entry(void __iomem *smbios_start,
         void __iomem *p_max;
 
         if (!smbios_table || !curr)
-                return(NULL);
+                return NULL;
 
-        // set p_max to the end of the table
+        /* set p_max to the end of the table */
         p_max = smbios_start + readw(smbios_table + ST_LENGTH);
 
         p_temp = curr;
@@ -253,20 +226,19 @@ static void __iomem *get_subsequent_smbios_entry(void __iomem *smbios_start,
         while ((p_temp < p_max) && !bail) {
                 /* Look for the double NULL terminator
                  * The first condition is the previous byte
-                 * and the second is the curr */
-                if (!previous_byte && !(readb(p_temp))) {
+                 * and the second is the curr
+                 */
+                if (!previous_byte && !(readb(p_temp)))
                         bail = 1;
-                }
 
                 previous_byte = readb(p_temp);
                 p_temp++;
         }
 
-        if (p_temp < p_max) {
+        if (p_temp < p_max)
                 return p_temp;
-        } else {
+        else
                 return NULL;
-        }
 }
 
 
@@ -292,21 +264,18 @@ static void __iomem *get_SMBIOS_entry(void __iomem *smbios_start,
         if (!smbios_table)
                 return NULL;
 
-        if (!previous) {                  
+        if (!previous)
                 previous = smbios_start;
-        } else {
+        else
                 previous = get_subsequent_smbios_entry(smbios_start,
                                         smbios_table, previous);
-        }
 
-        while (previous) {
-                if (readb(previous + SMBIOS_GENERIC_TYPE) != type) {
+        while (previous)
+                if (readb(previous + SMBIOS_GENERIC_TYPE) != type)
                         previous = get_subsequent_smbios_entry(smbios_start,
                                                 smbios_table, previous);
-                } else {
+                else
                         break;
-                }
-        }
 
         return previous;
 }
@@ -322,144 +291,6 @@ static void release_slot(struct hotplug_slot *hotplug_slot)
         kfree(slot);
 }
 
-#define SLOT_NAME_SIZE 10
-
-static int ctrl_slot_setup(struct controller *ctrl,
-                        void __iomem *smbios_start,
-                        void __iomem *smbios_table)
-{
-        struct slot *slot;
-        struct hotplug_slot *hotplug_slot;
-        struct hotplug_slot_info *hotplug_slot_info;
-        u8 number_of_slots;
-        u8 slot_device;
-        u8 slot_number;
-        u8 ctrl_slot;
-        u32 tempdword;
-        char name[SLOT_NAME_SIZE];
-        void __iomem *slot_entry= NULL;
-        int result = -ENOMEM;
-
-        dbg("%s\n", __func__);
-
-        tempdword = readl(ctrl->hpc_reg + INT_INPUT_CLEAR);
-
-        number_of_slots = readb(ctrl->hpc_reg + SLOT_MASK) & 0x0F;
-        slot_device = readb(ctrl->hpc_reg + SLOT_MASK) >> 4;
-        slot_number = ctrl->first_slot;
-
-        while (number_of_slots) {
-                slot = kzalloc(sizeof(*slot), GFP_KERNEL);
-                if (!slot)
-                        goto error;
-
-                slot->hotplug_slot = kzalloc(sizeof(*(slot->hotplug_slot)),
-                                                GFP_KERNEL);
-                if (!slot->hotplug_slot)
-                        goto error_slot;
-                hotplug_slot = slot->hotplug_slot;
-
-                hotplug_slot->info =
-                                kzalloc(sizeof(*(hotplug_slot->info)),
-                                                        GFP_KERNEL);
-                if (!hotplug_slot->info)
-                        goto error_hpslot;
-                hotplug_slot_info = hotplug_slot->info;
-
-                slot->ctrl = ctrl;
-                slot->bus = ctrl->bus;
-                slot->device = slot_device;
-                slot->number = slot_number;
-                dbg("slot->number = %u\n", slot->number);
-
-                slot_entry = get_SMBIOS_entry(smbios_start, smbios_table, 9,
-                                        slot_entry);
-
-                while (slot_entry && (readw(slot_entry + SMBIOS_SLOT_NUMBER) !=
-                                slot->number)) {
-                        slot_entry = get_SMBIOS_entry(smbios_start,
-                                                smbios_table, 9, slot_entry);
-                }
-
-                slot->p_sm_slot = slot_entry;
-
-                init_timer(&slot->task_event);
-                slot->task_event.expires = jiffies + 5 * HZ;
-                slot->task_event.function = cpqhp_pushbutton_thread;
-
-                //FIXME: these capabilities aren't used but if they are
-                //       they need to be correctly implemented
-                slot->capabilities |= PCISLOT_REPLACE_SUPPORTED;
-                slot->capabilities |= PCISLOT_INTERLOCK_SUPPORTED;
-
-                if (is_slot64bit(slot))
-                        slot->capabilities |= PCISLOT_64_BIT_SUPPORTED;
-                if (is_slot66mhz(slot))
-                        slot->capabilities |= PCISLOT_66_MHZ_SUPPORTED;
-                if (ctrl->speed == PCI_SPEED_66MHz)
-                        slot->capabilities |= PCISLOT_66_MHZ_OPERATION;
-
-                ctrl_slot =
-                        slot_device - (readb(ctrl->hpc_reg + SLOT_MASK) >> 4);
-
-                // Check presence
-                slot->capabilities |=
-                        ((((~tempdword) >> 23) |
-                         ((~tempdword) >> 15)) >> ctrl_slot) & 0x02;
-                // Check the switch state
-                slot->capabilities |=
-                        ((~tempdword & 0xFF) >> ctrl_slot) & 0x01;
-                // Check the slot enable
-                slot->capabilities |=
-                        ((read_slot_enable(ctrl) << 2) >> ctrl_slot) & 0x04;
-
-                /* register this slot with the hotplug pci core */
-                hotplug_slot->release = &release_slot;
-                hotplug_slot->private = slot;
-                snprintf(name, SLOT_NAME_SIZE, "%u", slot->number);
-                hotplug_slot->ops = &cpqphp_hotplug_slot_ops;
-
-                hotplug_slot_info->power_status = get_slot_enabled(ctrl, slot);
-                hotplug_slot_info->attention_status =
-                        cpq_get_attention_status(ctrl, slot);
-                hotplug_slot_info->latch_status =
-                        cpq_get_latch_status(ctrl, slot);
-                hotplug_slot_info->adapter_status =
-                        get_presence_status(ctrl, slot);
-                
-                dbg("registering bus %d, dev %d, number %d, "
-                                "ctrl->slot_device_offset %d, slot %d\n",
-                                slot->bus, slot->device,
-                                slot->number, ctrl->slot_device_offset,
-                                slot_number);
-                result = pci_hp_register(hotplug_slot,
-                                         ctrl->pci_dev->bus,
-                                         slot->device,
-                                         name);
-                if (result) {
-                        err("pci_hp_register failed with error %d\n", result);
-                        goto error_info;
-                }
-                
-                slot->next = ctrl->slot;
-                ctrl->slot = slot;
-
-                number_of_slots--;
-                slot_device++;
-                slot_number++;
-        }
-
-        return 0;
-error_info:
-        kfree(hotplug_slot_info);
-error_hpslot:
-        kfree(hotplug_slot);
-error_slot:
-        kfree(slot);
-error:
-        return result;
-}
-
 static int ctrl_slot_cleanup (struct controller * ctrl)
 {
         struct slot *old_slot, *next_slot;
@@ -476,36 +307,32 @@ static int ctrl_slot_cleanup (struct controller * ctrl)
 
         cpqhp_remove_debugfs_files(ctrl);
 
-        //Free IRQ associated with hot plug device
+        /* Free IRQ associated with hot plug device */
         free_irq(ctrl->interrupt, ctrl);
-        //Unmap the memory
+        /* Unmap the memory */
         iounmap(ctrl->hpc_reg);
-        //Finally reclaim PCI mem
+        /* Finally reclaim PCI mem */
         release_mem_region(pci_resource_start(ctrl->pci_dev, 0),
                            pci_resource_len(ctrl->pci_dev, 0));
 
-        return(0);
+        return 0;
 }
 
 
-//============================================================================
-// function:    get_slot_mapping
-//
-// Description: Attempts to determine a logical slot mapping for a PCI
-//              device.  Won't work for more than one PCI-PCI bridge
-//              in a slot.
-//
-// Input:       u8 bus_num - bus number of PCI device
-//              u8 dev_num - device number of PCI device
-//              u8 *slot - Pointer to u8 where slot number will
-//                      be returned
-//
-// Output:      SUCCESS or FAILURE
-//=============================================================================
+/**
+ * get_slot_mapping - determine logical slot mapping for PCI device
+ *
+ * Won't work for more than one PCI-PCI bridge in a slot.
+ *
+ * @bus_num - bus number of PCI device
+ * @dev_num - device number of PCI device
+ * @slot - Pointer to u8 where slot number will be returned
+ *
+ * Output:      SUCCESS or FAILURE
+ */
 static int
 get_slot_mapping(struct pci_bus *bus, u8 bus_num, u8 dev_num, u8 *slot)
 {
-        struct irq_routing_table *PCIIRQRoutingInfoLength;
         u32 work;
         long len;
         long loop;
@@ -516,36 +343,25 @@ get_slot_mapping(struct pci_bus *bus, u8 bus_num, u8 dev_num, u8 *slot)
 
         bridgeSlot = 0xFF;
 
-        PCIIRQRoutingInfoLength = pcibios_get_irq_routing_table();
-        if (!PCIIRQRoutingInfoLength)
-                return -1;
-
-        len = (PCIIRQRoutingInfoLength->size -
-               sizeof(struct irq_routing_table)) / sizeof(struct irq_info);
-        // Make sure I got at least one entry
-        if (len == 0) {
-                kfree(PCIIRQRoutingInfoLength);
-                return -1;
-        }
-
+        len = cpqhp_routing_table_length();
         for (loop = 0; loop < len; ++loop) {
-                tbus = PCIIRQRoutingInfoLength->slots[loop].bus;
-                tdevice = PCIIRQRoutingInfoLength->slots[loop].devfn >> 3;
-                tslot = PCIIRQRoutingInfoLength->slots[loop].slot;
+                tbus = cpqhp_routing_table->slots[loop].bus;
+                tdevice = cpqhp_routing_table->slots[loop].devfn >> 3;
+                tslot = cpqhp_routing_table->slots[loop].slot;
 
                 if ((tbus == bus_num) && (tdevice == dev_num)) {
                         *slot = tslot;
-                        kfree(PCIIRQRoutingInfoLength);
                         return 0;
                 } else {
                         /* Did not get a match on the target PCI device. Check
-                         * if the current IRQ table entry is a PCI-to-PCI bridge
-                         * device.  If so, and it's secondary bus matches the
-                         * bus number for the target device, I need to save the
-                         * bridge's slot number.  If I can not find an entry for
-                         * the target device, I will have to assume it's on the
-                         * other side of the bridge, and assign it the bridge's
-                         * slot. */
+                         * if the current IRQ table entry is a PCI-to-PCI
+                         * bridge device.  If so, and it's secondary bus
+                         * matches the bus number for the target device, I need
+                         * to save the bridge's slot number.  If I can not find
+                         * an entry for the target device, I will have to
+                         * assume it's on the other side of the bridge, and
+                         * assign it the bridge's slot.
+                         */
                         bus->number = tbus;
                         pci_bus_read_config_dword(bus, PCI_DEVFN(tdevice, 0),
                                                 PCI_CLASS_REVISION, &work);
@@ -555,25 +371,23 @@ get_slot_mapping(struct pci_bus *bus, u8 bus_num, u8 dev_num, u8 *slot)
                                                         PCI_DEVFN(tdevice, 0),
                                                         PCI_PRIMARY_BUS, &work);
                                 // See if bridge's secondary bus matches target bus.
-                                if (((work >> 8) & 0x000000FF) == (long) bus_num) {
+                                if (((work >> 8) & 0x000000FF) == (long) bus_num)
                                         bridgeSlot = tslot;
-                                }
                         }
                 }
 
         }
 
-        // If we got here, we didn't find an entry in the IRQ mapping table 
-        // for the target PCI device.  If we did determine that the target 
-        // device is on the other side of a PCI-to-PCI bridge, return the 
-        // slot number for the bridge.
+        /* If we got here, we didn't find an entry in the IRQ mapping table for
+         * the target PCI device.  If we did determine that the target device
+         * is on the other side of a PCI-to-PCI bridge, return the slot number
+         * for the bridge.
+         */
         if (bridgeSlot != 0xFF) {
                 *slot = bridgeSlot;
-                kfree(PCIIRQRoutingInfoLength);
                 return 0;
         }
-        kfree(PCIIRQRoutingInfoLength);
-        // Couldn't find an entry in the routing table for this PCI device
+        /* Couldn't find an entry in the routing table for this PCI device */
         return -1;
 }
 
@@ -591,32 +405,32 @@ cpqhp_set_attention_status(struct controller *ctrl, struct pci_func *func,
         u8 hp_slot;
 
         if (func == NULL)
-                return(1);
+                return 1;
 
         hp_slot = func->device - ctrl->slot_device_offset;
 
-        // Wait for exclusive access to hardware
+        /* Wait for exclusive access to hardware */
         mutex_lock(&ctrl->crit_sect);
 
-        if (status == 1) {
+        if (status == 1)
                 amber_LED_on (ctrl, hp_slot);
-        } else if (status == 0) {
+        else if (status == 0)
                 amber_LED_off (ctrl, hp_slot);
-        } else {
-                // Done with exclusive hardware access
+        else {
+                /* Done with exclusive hardware access */
                 mutex_unlock(&ctrl->crit_sect);
-                return(1);
+                return 1;
         }
 
         set_SOGO(ctrl);
 
-        // Wait for SOBS to be unset
+        /* Wait for SOBS to be unset */
         wait_for_ctrl_irq (ctrl);
 
-        // Done with exclusive hardware access
+        /* Done with exclusive hardware access */
         mutex_unlock(&ctrl->crit_sect);
 
-        return(0);
+        return 0;
 }
 
 
@@ -719,7 +533,7 @@ static int hardware_test(struct hotplug_slot *hotplug_slot, u32 value)
 
         dbg("%s - physical_slot = %s\n", __func__, slot_name(slot));
 
-        return cpqhp_hardware_test(ctrl, value);        
+        return cpqhp_hardware_test(ctrl, value);
 }
 
 
@@ -738,7 +552,7 @@ static int get_attention_status(struct hotplug_slot *hotplug_slot, u8 *value)
 {
         struct slot *slot = hotplug_slot->private;
         struct controller *ctrl = slot->ctrl;
-        
+
         dbg("%s - physical_slot = %s\n", __func__, slot_name(slot));
 
         *value = cpq_get_attention_status(ctrl, slot);
@@ -793,6 +607,230 @@ static int get_cur_bus_speed (struct hotplug_slot *hotplug_slot, enum pci_bus_sp
         return 0;
 }
 
+static struct hotplug_slot_ops cpqphp_hotplug_slot_ops = {
+        .set_attention_status = set_attention_status,
+        .enable_slot =          process_SI,
+        .disable_slot =         process_SS,
+        .hardware_test =        hardware_test,
+        .get_power_status =     get_power_status,
+        .get_attention_status = get_attention_status,
+        .get_latch_status =     get_latch_status,
+        .get_adapter_status =   get_adapter_status,
+        .get_max_bus_speed =    get_max_bus_speed,
+        .get_cur_bus_speed =    get_cur_bus_speed,
+};
+
+#define SLOT_NAME_SIZE 10
+
+static int ctrl_slot_setup(struct controller *ctrl,
+                        void __iomem *smbios_start,
+                        void __iomem *smbios_table)
+{
+        struct slot *slot;
+        struct hotplug_slot *hotplug_slot;
+        struct hotplug_slot_info *hotplug_slot_info;
+        u8 number_of_slots;
+        u8 slot_device;
+        u8 slot_number;
+        u8 ctrl_slot;
+        u32 tempdword;
+        char name[SLOT_NAME_SIZE];
+        void __iomem *slot_entry= NULL;
+        int result = -ENOMEM;
+
+        dbg("%s\n", __func__);
+
+        tempdword = readl(ctrl->hpc_reg + INT_INPUT_CLEAR);
+
+        number_of_slots = readb(ctrl->hpc_reg + SLOT_MASK) & 0x0F;
+        slot_device = readb(ctrl->hpc_reg + SLOT_MASK) >> 4;
+        slot_number = ctrl->first_slot;
+
+        while (number_of_slots) {
+                slot = kzalloc(sizeof(*slot), GFP_KERNEL);
+                if (!slot)
+                        goto error;
+
+                slot->hotplug_slot = kzalloc(sizeof(*(slot->hotplug_slot)),
+                                                GFP_KERNEL);
+                if (!slot->hotplug_slot)
+                        goto error_slot;
+                hotplug_slot = slot->hotplug_slot;
+
+                hotplug_slot->info = kzalloc(sizeof(*(hotplug_slot->info)),
+                                                        GFP_KERNEL);
+                if (!hotplug_slot->info)
+                        goto error_hpslot;
+                hotplug_slot_info = hotplug_slot->info;
+
+                slot->ctrl = ctrl;
+                slot->bus = ctrl->bus;
+                slot->device = slot_device;
+                slot->number = slot_number;
+                dbg("slot->number = %u\n", slot->number);
+
+                slot_entry = get_SMBIOS_entry(smbios_start, smbios_table, 9,
+                                        slot_entry);
+
+                while (slot_entry && (readw(slot_entry + SMBIOS_SLOT_NUMBER) !=
+                                slot->number)) {
+                        slot_entry = get_SMBIOS_entry(smbios_start,
+                                                smbios_table, 9, slot_entry);
+                }
+
+                slot->p_sm_slot = slot_entry;
+
+                init_timer(&slot->task_event);
+                slot->task_event.expires = jiffies + 5 * HZ;
+                slot->task_event.function = cpqhp_pushbutton_thread;
+
+                /*FIXME: these capabilities aren't used but if they are
+                 *       they need to be correctly implemented
+                 */
+                slot->capabilities |= PCISLOT_REPLACE_SUPPORTED;
+                slot->capabilities |= PCISLOT_INTERLOCK_SUPPORTED;
+
+                if (is_slot64bit(slot))
+                        slot->capabilities |= PCISLOT_64_BIT_SUPPORTED;
+                if (is_slot66mhz(slot))
+                        slot->capabilities |= PCISLOT_66_MHZ_SUPPORTED;
+                if (ctrl->speed == PCI_SPEED_66MHz)
+                        slot->capabilities |= PCISLOT_66_MHZ_OPERATION;
+
+                ctrl_slot =
+                        slot_device - (readb(ctrl->hpc_reg + SLOT_MASK) >> 4);
+
+                /* Check presence */
+                slot->capabilities |=
+                        ((((~tempdword) >> 23) |
+                         ((~tempdword) >> 15)) >> ctrl_slot) & 0x02;
+                /* Check the switch state */
+                slot->capabilities |=
+                        ((~tempdword & 0xFF) >> ctrl_slot) & 0x01;
+                /* Check the slot enable */
+                slot->capabilities |=
+                        ((read_slot_enable(ctrl) << 2) >> ctrl_slot) & 0x04;
+
+                /* register this slot with the hotplug pci core */
+                hotplug_slot->release = &release_slot;
+                hotplug_slot->private = slot;
+                snprintf(name, SLOT_NAME_SIZE, "%u", slot->number);
+                hotplug_slot->ops = &cpqphp_hotplug_slot_ops;
+
+                hotplug_slot_info->power_status = get_slot_enabled(ctrl, slot);
+                hotplug_slot_info->attention_status =
+                        cpq_get_attention_status(ctrl, slot);
+                hotplug_slot_info->latch_status =
+                        cpq_get_latch_status(ctrl, slot);
+                hotplug_slot_info->adapter_status =
+                        get_presence_status(ctrl, slot);
+
+                dbg("registering bus %d, dev %d, number %d, "
+                                "ctrl->slot_device_offset %d, slot %d\n",
+                                slot->bus, slot->device,
+                                slot->number, ctrl->slot_device_offset,
+                                slot_number);
+                result = pci_hp_register(hotplug_slot,
+                                         ctrl->pci_dev->bus,
+                                         slot->device,
+                                         name);
+                if (result) {
+                        err("pci_hp_register failed with error %d\n", result);
+                        goto error_info;
+                }
+
+                slot->next = ctrl->slot;
+                ctrl->slot = slot;
+
+                number_of_slots--;
+                slot_device++;
+                slot_number++;
+        }
+
+        return 0;
+error_info:
+        kfree(hotplug_slot_info);
+error_hpslot:
+        kfree(hotplug_slot);
+error_slot:
+        kfree(slot);
+error:
+        return result;
+}
+
+static int one_time_init(void)
+{
+        int loop;
+        int retval = 0;
+
+        if (initialized)
+                return 0;
+
+        power_mode = 0;
+
+        retval = init_cpqhp_routing_table();
+        if (retval)
+                goto error;
+
+        if (cpqhp_debug)
+                pci_print_IRQ_route();
+
+        dbg("Initialize + Start the notification mechanism \n");
+
+        retval = cpqhp_event_start_thread();
+        if (retval)
+                goto error;
+
+        dbg("Initialize slot lists\n");
+        for (loop = 0; loop < 256; loop++)
+                cpqhp_slot_list[loop] = NULL;
+
+        /* FIXME: We also need to hook the NMI handler eventually.
+         * this also needs to be worked with Christoph
+         * register_NMI_handler();
+         */
+        /* Map rom address */
+        cpqhp_rom_start = ioremap(ROM_PHY_ADDR, ROM_PHY_LEN);
+        if (!cpqhp_rom_start) {
+                err ("Could not ioremap memory region for ROM\n");
+                retval = -EIO;
+                goto error;
+        }
+
+        /* Now, map the int15 entry point if we are on compaq specific
+         * hardware
+         */
+        compaq_nvram_init(cpqhp_rom_start);
+
+        /* Map smbios table entry point structure */
+        smbios_table = detect_SMBIOS_pointer(cpqhp_rom_start,
+                                        cpqhp_rom_start + ROM_PHY_LEN);
+        if (!smbios_table) {
+                err ("Could not find the SMBIOS pointer in memory\n");
+                retval = -EIO;
+                goto error_rom_start;
+        }
+
+        smbios_start = ioremap(readl(smbios_table + ST_ADDRESS),
+                                        readw(smbios_table + ST_LENGTH));
+        if (!smbios_start) {
+                err ("Could not ioremap memory region taken from SMBIOS values\n");
+                retval = -EIO;
+                goto error_smbios_start;
+        }
+
+        initialized = 1;
+
+        return retval;
+
+error_smbios_start:
+        iounmap(smbios_start);
+error_rom_start:
+        iounmap(cpqhp_rom_start);
+error:
+        return retval;
+}
+
 static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
         u8 num_of_slots = 0;
@@ -815,7 +853,9 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
                 return err;
         }
 
-        // Need to read VID early b/c it's used to differentiate CPQ and INTC discovery
+        /* Need to read VID early b/c it's used to differentiate CPQ and INTC
+         * discovery
+         */
         rc = pci_read_config_word(pdev, PCI_VENDOR_ID, &vendor_id);
         if (rc || ((vendor_id != PCI_VENDOR_ID_COMPAQ) && (vendor_id != PCI_VENDOR_ID_INTEL))) {
                 err(msg_HPC_non_compaq_or_intel);
@@ -832,217 +872,209 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
         }
 
         /* Check for the proper subsytem ID's
-         * Intel uses a different SSID programming model than Compaq.  
+         * Intel uses a different SSID programming model than Compaq.
          * For Intel, each SSID bit identifies a PHP capability.
          * Also Intel HPC's may have RID=0.
          */
-        if ((pdev->revision > 2) || (vendor_id == PCI_VENDOR_ID_INTEL)) {
-                // TODO: This code can be made to support non-Compaq or Intel subsystem IDs
-                rc = pci_read_config_word(pdev, PCI_SUBSYSTEM_VENDOR_ID, &subsystem_vid);
-                if (rc) {
-                        err("%s : pci_read_config_word failed\n", __func__);
-                        goto err_disable_device;
-                }
-                dbg("Subsystem Vendor ID: %x\n", subsystem_vid);
-                if ((subsystem_vid != PCI_VENDOR_ID_COMPAQ) && (subsystem_vid != PCI_VENDOR_ID_INTEL)) {
-                        err(msg_HPC_non_compaq_or_intel);
-                        rc = -ENODEV;
-                        goto err_disable_device;
-                }
+        if ((pdev->revision <= 2) && (vendor_id != PCI_VENDOR_ID_INTEL)) {
+                err(msg_HPC_not_supported);
+                return -ENODEV;
+        }
 
-                ctrl = kzalloc(sizeof(struct controller), GFP_KERNEL);
-                if (!ctrl) {
-                        err("%s : out of memory\n", __func__);
-                        rc = -ENOMEM;
-                        goto err_disable_device;
-                }
+        /* TODO: This code can be made to support non-Compaq or Intel
+         * subsystem IDs
+         */
+        rc = pci_read_config_word(pdev, PCI_SUBSYSTEM_VENDOR_ID, &subsystem_vid);
+        if (rc) {
+                err("%s : pci_read_config_word failed\n", __func__);
+                goto err_disable_device;
+        }
+        dbg("Subsystem Vendor ID: %x\n", subsystem_vid);
+        if ((subsystem_vid != PCI_VENDOR_ID_COMPAQ) && (subsystem_vid != PCI_VENDOR_ID_INTEL)) {
+                err(msg_HPC_non_compaq_or_intel);
+                rc = -ENODEV;
+                goto err_disable_device;
+        }
 
-                rc = pci_read_config_word(pdev, PCI_SUBSYSTEM_ID, &subsystem_deviceid);
-                if (rc) {
-                        err("%s : pci_read_config_word failed\n", __func__);
-                        goto err_free_ctrl;
-                }
+        ctrl = kzalloc(sizeof(struct controller), GFP_KERNEL);
+        if (!ctrl) {
+                err("%s : out of memory\n", __func__);
+                rc = -ENOMEM;
+                goto err_disable_device;
+        }
 
-                info("Hot Plug Subsystem Device ID: %x\n", subsystem_deviceid);
-
-                /* Set Vendor ID, so it can be accessed later from other functions */
-                ctrl->vendor_id = vendor_id;
-
-                switch (subsystem_vid) {
-                        case PCI_VENDOR_ID_COMPAQ:
-                                if (pdev->revision >= 0x13) { /* CIOBX */
-                                        ctrl->push_flag = 1;
-                                        ctrl->slot_switch_type = 1;
-                                        ctrl->push_button = 1;
-                                        ctrl->pci_config_space = 1;
-                                        ctrl->defeature_PHP = 1;
-                                        ctrl->pcix_support = 1;
-                                        ctrl->pcix_speed_capability = 1;
-                                        pci_read_config_byte(pdev, 0x41, &bus_cap);
-                                        if (bus_cap & 0x80) {
-                                                dbg("bus max supports 133MHz PCI-X\n");
-                                                ctrl->speed_capability = PCI_SPEED_133MHz_PCIX;
-                                                break;
-                                        }
-                                        if (bus_cap & 0x40) {
-                                                dbg("bus max supports 100MHz PCI-X\n");
-                                                ctrl->speed_capability = PCI_SPEED_100MHz_PCIX;
-                                                break;
-                                        }
-                                        if (bus_cap & 20) {
-                                                dbg("bus max supports 66MHz PCI-X\n");
-                                                ctrl->speed_capability = PCI_SPEED_66MHz_PCIX;
-                                                break;
-                                        }
-                                        if (bus_cap & 10) {
-                                                dbg("bus max supports 66MHz PCI\n");
-                                                ctrl->speed_capability = PCI_SPEED_66MHz;
-                                                break;
-                                        }
-
-                                        break;
-                                }
-
-                                switch (subsystem_deviceid) {
-                                        case PCI_SUB_HPC_ID:
-                                                /* Original 6500/7000 implementation */
-                                                ctrl->slot_switch_type = 1;
-                                                ctrl->speed_capability = PCI_SPEED_33MHz;
-                                                ctrl->push_button = 0;
-                                                ctrl->pci_config_space = 1;
-                                                ctrl->defeature_PHP = 1;
-                                                ctrl->pcix_support = 0;
-                                                ctrl->pcix_speed_capability = 0;
-                                                break;
-                                        case PCI_SUB_HPC_ID2:
-                                                /* First Pushbutton implementation */
-                                                ctrl->push_flag = 1;
-                                                ctrl->slot_switch_type = 1;
-                                                ctrl->speed_capability = PCI_SPEED_33MHz;
-                                                ctrl->push_button = 1;
-                                                ctrl->pci_config_space = 1;
-                                                ctrl->defeature_PHP = 1;
-                                                ctrl->pcix_support = 0;
-                                                ctrl->pcix_speed_capability = 0;
-                                                break;
-                                        case PCI_SUB_HPC_ID_INTC:
-                                                /* Third party (6500/7000) */
-                                                ctrl->slot_switch_type = 1;
-                                                ctrl->speed_capability = PCI_SPEED_33MHz;
-                                                ctrl->push_button = 0;
-                                                ctrl->pci_config_space = 1;
-                                                ctrl->defeature_PHP = 1;
-                                                ctrl->pcix_support = 0;
-                                                ctrl->pcix_speed_capability = 0;
-                                                break;
-                                        case PCI_SUB_HPC_ID3:
-                                                /* First 66 Mhz implementation */
-                                                ctrl->push_flag = 1;
-                                                ctrl->slot_switch_type = 1;
-                                                ctrl->speed_capability = PCI_SPEED_66MHz;
-                                                ctrl->push_button = 1;
-                                                ctrl->pci_config_space = 1;
-                                                ctrl->defeature_PHP = 1;
-                                                ctrl->pcix_support = 0;
-                                                ctrl->pcix_speed_capability = 0;
-                                                break;
-                                        case PCI_SUB_HPC_ID4:
-                                                /* First PCI-X implementation, 100MHz */
-                                                ctrl->push_flag = 1;
-                                                ctrl->slot_switch_type = 1;
-                                                ctrl->speed_capability = PCI_SPEED_100MHz_PCIX;
-                                                ctrl->push_button = 1;
-                                                ctrl->pci_config_space = 1;
-                                                ctrl->defeature_PHP = 1;
-                                                ctrl->pcix_support = 1;
-                                                ctrl->pcix_speed_capability = 0;        
-                                                break;
-                                        default:
-                                                err(msg_HPC_not_supported);
-                                                rc = -ENODEV;
-                                                goto err_free_ctrl;
-                                }
-                                break;
+        rc = pci_read_config_word(pdev, PCI_SUBSYSTEM_ID, &subsystem_deviceid);
+        if (rc) {
+                err("%s : pci_read_config_word failed\n", __func__);
+                goto err_free_ctrl;
+        }
 
-                        case PCI_VENDOR_ID_INTEL:
-                                /* Check for speed capability (0=33, 1=66) */
-                                if (subsystem_deviceid & 0x0001) {
-                                        ctrl->speed_capability = PCI_SPEED_66MHz;
-                                } else {
-                                        ctrl->speed_capability = PCI_SPEED_33MHz;
-                                }
-
-                                /* Check for push button */
-                                if (subsystem_deviceid & 0x0002) {
-                                        /* no push button */
-                                        ctrl->push_button = 0;
-                                } else {
-                                        /* push button supported */
-                                        ctrl->push_button = 1;
-                                }
-
-                                /* Check for slot switch type (0=mechanical, 1=not mechanical) */
-                                if (subsystem_deviceid & 0x0004) {
-                                        /* no switch */
-                                        ctrl->slot_switch_type = 0;
-                                } else {
-                                        /* switch */
-                                        ctrl->slot_switch_type = 1;
-                                }
-
-                                /* PHP Status (0=De-feature PHP, 1=Normal operation) */
-                                if (subsystem_deviceid & 0x0008) {
-                                        ctrl->defeature_PHP = 1;        // PHP supported
-                                } else {
-                                        ctrl->defeature_PHP = 0;        // PHP not supported
-                                }
-
-                                /* Alternate Base Address Register Interface (0=not supported, 1=supported) */
-                                if (subsystem_deviceid & 0x0010) {
-                                        ctrl->alternate_base_address = 1;       // supported
-                                } else {
-                                        ctrl->alternate_base_address = 0;       // not supported
-                                }
-
-                                /* PCI Config Space Index (0=not supported, 1=supported) */
-                                if (subsystem_deviceid & 0x0020) {
-                                        ctrl->pci_config_space = 1;             // supported
-                                } else {
-                                        ctrl->pci_config_space = 0;             // not supported
-                                }
-
-                                /* PCI-X support */
-                                if (subsystem_deviceid & 0x0080) {
-                                        /* PCI-X capable */
-                                        ctrl->pcix_support = 1;
-                                        /* Frequency of operation in PCI-X mode */
-                                        if (subsystem_deviceid & 0x0040) {
-                                                /* 133MHz PCI-X if bit 7 is 1 */
-                                                ctrl->pcix_speed_capability = 1;
-                                        } else {
-                                                /* 100MHz PCI-X if bit 7 is 1 and bit 0 is 0, */
-                                                /* 66MHz PCI-X if bit 7 is 1 and bit 0 is 1 */
-                                                ctrl->pcix_speed_capability = 0;
-                                        }
-                                } else {
-                                        /* Conventional PCI */
-                                        ctrl->pcix_support = 0;
-                                        ctrl->pcix_speed_capability = 0;
-                                }
+        info("Hot Plug Subsystem Device ID: %x\n", subsystem_deviceid);
+
+        /* Set Vendor ID, so it can be accessed later from other
+         * functions
+         */
+        ctrl->vendor_id = vendor_id;
+
+        switch (subsystem_vid) {
+        case PCI_VENDOR_ID_COMPAQ:
+                if (pdev->revision >= 0x13) { /* CIOBX */
+                        ctrl->push_flag = 1;
+                        ctrl->slot_switch_type = 1;
+                        ctrl->push_button = 1;
+                        ctrl->pci_config_space = 1;
+                        ctrl->defeature_PHP = 1;
+                        ctrl->pcix_support = 1;
+                        ctrl->pcix_speed_capability = 1;
+                        pci_read_config_byte(pdev, 0x41, &bus_cap);
+                        if (bus_cap & 0x80) {
+                                dbg("bus max supports 133MHz PCI-X\n");
+                                ctrl->speed_capability = PCI_SPEED_133MHz_PCIX;
                                 break;
+                        }
+                        if (bus_cap & 0x40) {
+                                dbg("bus max supports 100MHz PCI-X\n");
+                                ctrl->speed_capability = PCI_SPEED_100MHz_PCIX;
+                                break;
+                        }
+                        if (bus_cap & 20) {
+                                dbg("bus max supports 66MHz PCI-X\n");
+                                ctrl->speed_capability = PCI_SPEED_66MHz_PCIX;
+                                break;
+                        }
+                        if (bus_cap & 10) {
+                                dbg("bus max supports 66MHz PCI\n");
+                                ctrl->speed_capability = PCI_SPEED_66MHz;
+                                break;
+                        }
+
+                        break;
+                }
 
-                        default:
-                                err(msg_HPC_not_supported);
-                                rc = -ENODEV;
-                                goto err_free_ctrl;
+                switch (subsystem_deviceid) {
+                case PCI_SUB_HPC_ID:
+                        /* Original 6500/7000 implementation */
+                        ctrl->slot_switch_type = 1;
+                        ctrl->speed_capability = PCI_SPEED_33MHz;
+                        ctrl->push_button = 0;
+                        ctrl->pci_config_space = 1;
+                        ctrl->defeature_PHP = 1;
+                        ctrl->pcix_support = 0;
+                        ctrl->pcix_speed_capability = 0;
+                        break;
+                case PCI_SUB_HPC_ID2:
+                        /* First Pushbutton implementation */
+                        ctrl->push_flag = 1;
+                        ctrl->slot_switch_type = 1;
+                        ctrl->speed_capability = PCI_SPEED_33MHz;
+                        ctrl->push_button = 1;
+                        ctrl->pci_config_space = 1;
+                        ctrl->defeature_PHP = 1;
+                        ctrl->pcix_support = 0;
+                        ctrl->pcix_speed_capability = 0;
+                        break;
+                case PCI_SUB_HPC_ID_INTC:
+                        /* Third party (6500/7000) */
+                        ctrl->slot_switch_type = 1;
+                        ctrl->speed_capability = PCI_SPEED_33MHz;
+                        ctrl->push_button = 0;
+                        ctrl->pci_config_space = 1;
+                        ctrl->defeature_PHP = 1;
+                        ctrl->pcix_support = 0;
+                        ctrl->pcix_speed_capability = 0;
+                        break;
+                case PCI_SUB_HPC_ID3:
+                        /* First 66 Mhz implementation */
+                        ctrl->push_flag = 1;
+                        ctrl->slot_switch_type = 1;
+                        ctrl->speed_capability = PCI_SPEED_66MHz;
+                        ctrl->push_button = 1;
+                        ctrl->pci_config_space = 1;
+                        ctrl->defeature_PHP = 1;
+                        ctrl->pcix_support = 0;
+                        ctrl->pcix_speed_capability = 0;
+                        break;
+                case PCI_SUB_HPC_ID4:
+                        /* First PCI-X implementation, 100MHz */
+                        ctrl->push_flag = 1;
+                        ctrl->slot_switch_type = 1;
+                        ctrl->speed_capability = PCI_SPEED_100MHz_PCIX;
+                        ctrl->push_button = 1;
+                        ctrl->pci_config_space = 1;
+                        ctrl->defeature_PHP = 1;
+                        ctrl->pcix_support = 1;
+                        ctrl->pcix_speed_capability = 0;
+                        break;
+                default:
+                        err(msg_HPC_not_supported);
+                        rc = -ENODEV;
+                        goto err_free_ctrl;
                 }
+                break;
+
+        case PCI_VENDOR_ID_INTEL:
+                /* Check for speed capability (0=33, 1=66) */
+                if (subsystem_deviceid & 0x0001)
+                        ctrl->speed_capability = PCI_SPEED_66MHz;
+                else
+                        ctrl->speed_capability = PCI_SPEED_33MHz;
+
+                /* Check for push button */
+                if (subsystem_deviceid & 0x0002)
+                        ctrl->push_button = 0;
+                else
+                        ctrl->push_button = 1;
+
+                /* Check for slot switch type (0=mechanical, 1=not mechanical) */
+                if (subsystem_deviceid & 0x0004)
+                        ctrl->slot_switch_type = 0;
+                else
+                        ctrl->slot_switch_type = 1;
+
+                /* PHP Status (0=De-feature PHP, 1=Normal operation) */
+                if (subsystem_deviceid & 0x0008)
+                        ctrl->defeature_PHP = 1;        /* PHP supported */
+                else
+                        ctrl->defeature_PHP = 0;        /* PHP not supported */
+
+                /* Alternate Base Address Register Interface
+                 * (0=not supported, 1=supported)
+                 */
+                if (subsystem_deviceid & 0x0010)
+                        ctrl->alternate_base_address = 1;
+                else
+                        ctrl->alternate_base_address = 0;
+
+                /* PCI Config Space Index (0=not supported, 1=supported) */
+                if (subsystem_deviceid & 0x0020)
+                        ctrl->pci_config_space = 1;
+                else
+                        ctrl->pci_config_space = 0;
+
+                /* PCI-X support */
+                if (subsystem_deviceid & 0x0080) {
+                        ctrl->pcix_support = 1;
+                        if (subsystem_deviceid & 0x0040)
+                                /* 133MHz PCI-X if bit 7 is 1 */
+                                ctrl->pcix_speed_capability = 1;
+                        else
+                                /* 100MHz PCI-X if bit 7 is 1 and bit 0 is 0, */
+                                /* 66MHz PCI-X if bit 7 is 1 and bit 0 is 1 */
+                                ctrl->pcix_speed_capability = 0;
+                } else {
+                        /* Conventional PCI */
+                        ctrl->pcix_support = 0;
+                        ctrl->pcix_speed_capability = 0;
+                }
+                break;
 
-        } else {
+        default:
                 err(msg_HPC_not_supported);
-                return -ENODEV;
+                rc = -ENODEV;
+                goto err_free_ctrl;
         }
 
-        // Tell the user that we found one.
+        /* Tell the user that we found one. */
         info("Initializing the PCI hot plug controller residing on PCI bus %d\n",
                                         pdev->bus->number);
 
@@ -1087,7 +1119,7 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
         if (rc) {
                 goto err_free_bus;
         }
-        
+
         dbg("pdev = %p\n", pdev);
         dbg("pci resource start %llx\n", (unsigned long long)pci_resource_start(pdev, 0));
         dbg("pci resource len %llx\n", (unsigned long long)pci_resource_len(pdev, 0));
@@ -1109,7 +1141,7 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
                 goto err_free_mem_region;
         }
 
-        // Check for 66Mhz operation
+        /* Check for 66Mhz operation */
         ctrl->speed = get_controller_speed(ctrl);
 
 
@@ -1120,7 +1152,7 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
          *
          ********************************************************/
 
-        // find the physical slot number of the first hot plug slot
+        /* find the physical slot number of the first hot plug slot */
 
         /* Get slot won't work for devices behind bridges, but
          * in this case it will always be called for the "base"
@@ -1137,7 +1169,7 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
                 goto err_iounmap;
         }
 
-        // Store PCI Config Space for all devices on this bus
+        /* Store PCI Config Space for all devices on this bus */
         rc = cpqhp_save_config(ctrl, ctrl->bus, readb(ctrl->hpc_reg + SLOT_MASK));
         if (rc) {
                 err("%s: unable to save PCI configuration data, error %d\n",
@@ -1148,7 +1180,7 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
         /*
          * Get IO, memory, and IRQ resources for new devices
          */
-        // The next line is required for cpqhp_find_available_resources
+        /* The next line is required for cpqhp_find_available_resources */
         ctrl->interrupt = pdev->irq;
         if (ctrl->interrupt < 0x10) {
                 cpqhp_legacy_mode = 1;
@@ -1182,7 +1214,7 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
                         __func__, rc);
                 goto err_iounmap;
         }
-        
+
         /* Mask all general input interrupts */
         writel(0xFFFFFFFFL, ctrl->hpc_reg + INT_MASK);
 
@@ -1196,12 +1228,14 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
                 goto err_iounmap;
         }
 
-        /* Enable Shift Out interrupt and clear it, also enable SERR on power fault */
+        /* Enable Shift Out interrupt and clear it, also enable SERR on power
+         * fault
+         */
         temp_word = readw(ctrl->hpc_reg + MISC);
         temp_word |= 0x4006;
         writew(temp_word, ctrl->hpc_reg + MISC);
 
-        // Changed 05/05/97 to clear all interrupts at start
+        /* Changed 05/05/97 to clear all interrupts at start */
         writel(0xFFFFFFFFL, ctrl->hpc_reg + INT_INPUT_CLEAR);
 
         ctrl->ctrl_int_comp = readl(ctrl->hpc_reg + INT_INPUT_CLEAR);
@@ -1216,13 +1250,14 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
                 cpqhp_ctrl_list = ctrl;
         }
 
-        // turn off empty slots here unless command line option "ON" set
-        // Wait for exclusive access to hardware
+        /* turn off empty slots here unless command line option "ON" set
+         * Wait for exclusive access to hardware
+         */
         mutex_lock(&ctrl->crit_sect);
 
         num_of_slots = readb(ctrl->hpc_reg + SLOT_MASK) & 0x0F;
 
-        // find first device number for the ctrl
+        /* find first device number for the ctrl */
         device = readb(ctrl->hpc_reg + SLOT_MASK) >> 4;
 
         while (num_of_slots) {
@@ -1234,23 +1269,21 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
                 hp_slot = func->device - ctrl->slot_device_offset;
                 dbg("hp_slot: %d\n", hp_slot);
 
-                // We have to save the presence info for these slots
+                /* We have to save the presence info for these slots */
                 temp_word = ctrl->ctrl_int_comp >> 16;
                 func->presence_save = (temp_word >> hp_slot) & 0x01;
                 func->presence_save |= (temp_word >> (hp_slot + 7)) & 0x02;
 
-                if (ctrl->ctrl_int_comp & (0x1L << hp_slot)) {
+                if (ctrl->ctrl_int_comp & (0x1L << hp_slot))
                         func->switch_save = 0;
-                } else {
+                else
                         func->switch_save = 0x10;
-                }
 
-                if (!power_mode) {
+                if (!power_mode)
                         if (!func->is_a_board) {
                                 green_LED_off(ctrl, hp_slot);
                                 slot_disable(ctrl, hp_slot);
                         }
-                }
 
                 device++;
                 num_of_slots--;
@@ -1258,7 +1291,7 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
         if (!power_mode) {
                 set_SOGO(ctrl);
-                // Wait for SOBS to be unset
+                /* Wait for SOBS to be unset */
                 wait_for_ctrl_irq(ctrl);
         }
 
@@ -1269,7 +1302,7 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
                 goto err_free_irq;
         }
 
-        // Done with exclusive hardware access
+        /* Done with exclusive hardware access */
         mutex_unlock(&ctrl->crit_sect);
 
         cpqhp_create_debugfs_files(ctrl);
@@ -1291,77 +1324,6 @@ err_disable_device:
         return rc;
 }
 
-
-static int one_time_init(void)
-{
-        int loop;
-        int retval = 0;
-
-        if (initialized)
-                return 0;
-
-        power_mode = 0;
-
-        retval = pci_print_IRQ_route();
-        if (retval)
-                goto error;
-
-        dbg("Initialize + Start the notification mechanism \n");
-
-        retval = cpqhp_event_start_thread();
-        if (retval)
-                goto error;
-
-        dbg("Initialize slot lists\n");
-        for (loop = 0; loop < 256; loop++) {
-                cpqhp_slot_list[loop] = NULL;
-        }
-
-        // FIXME: We also need to hook the NMI handler eventually.
-        // this also needs to be worked with Christoph
-        // register_NMI_handler();
-
-        // Map rom address
-        cpqhp_rom_start = ioremap(ROM_PHY_ADDR, ROM_PHY_LEN);
-        if (!cpqhp_rom_start) {
-                err ("Could not ioremap memory region for ROM\n");
-                retval = -EIO;
-                goto error;
-        }
-        
-        /* Now, map the int15 entry point if we are on compaq specific hardware */
-        compaq_nvram_init(cpqhp_rom_start);
-        
-        /* Map smbios table entry point structure */
-        smbios_table = detect_SMBIOS_pointer(cpqhp_rom_start,
-                                        cpqhp_rom_start + ROM_PHY_LEN);
-        if (!smbios_table) {
-                err ("Could not find the SMBIOS pointer in memory\n");
-                retval = -EIO;
-                goto error_rom_start;
-        }
-
-        smbios_start = ioremap(readl(smbios_table + ST_ADDRESS),
-                                        readw(smbios_table + ST_LENGTH));
-        if (!smbios_start) {
-                err ("Could not ioremap memory region taken from SMBIOS values\n");
-                retval = -EIO;
-                goto error_smbios_start;
-        }
-
-        initialized = 1;
-
-        return retval;
-
-error_smbios_start:
-        iounmap(smbios_start);
-error_rom_start:
-        iounmap(cpqhp_rom_start);
-error:
-        return retval;
-}
-
-
 static void __exit unload_cpqphpd(void)
 {
         struct pci_func *next;
@@ -1381,10 +1343,10 @@ static void __exit unload_cpqphpd(void)
                 if (ctrl->hpc_reg) {
                         u16 misc;
                         rc = read_slot_enable (ctrl);
-                        
+
                         writeb(0, ctrl->hpc_reg + SLOT_SERR);
                         writel(0xFFFFFFC0L | ~rc, ctrl->hpc_reg + INT_MASK);
-                        
+
                         misc = readw(ctrl->hpc_reg + MISC);
                         misc &= 0xFFFD;
                         writew(misc, ctrl->hpc_reg + MISC);
@@ -1464,38 +1426,34 @@ static void __exit unload_cpqphpd(void)
                 }
         }
 
-        // Stop the notification mechanism
+        /* Stop the notification mechanism */
         if (initialized)
                 cpqhp_event_stop_thread();
 
-        //unmap the rom address
+        /* unmap the rom address */
         if (cpqhp_rom_start)
                 iounmap(cpqhp_rom_start);
         if (smbios_start)
                 iounmap(smbios_start);
 }
 
-
-
 static struct pci_device_id hpcd_pci_tbl[] = {
         {
         /* handle any PCI Hotplug controller */
         .class =        ((PCI_CLASS_SYSTEM_PCI_HOTPLUG << 8) | 0x00),
         .class_mask =   ~0,
-        
+
         /* no matter who makes it */
         .vendor =       PCI_ANY_ID,
         .device =       PCI_ANY_ID,
         .subvendor =    PCI_ANY_ID,
         .subdevice =    PCI_ANY_ID,
-        
+
         }, { /* end: all zeroes */ }
 };
 
 MODULE_DEVICE_TABLE(pci, hpcd_pci_tbl);
 
-
-
 static struct pci_driver cpqhpc_driver = {
         .name =         "compaq_pci_hotplug",
         .id_table =     hpcd_pci_tbl,
@@ -1503,8 +1461,6 @@ static struct pci_driver cpqhpc_driver = {
         /* remove:      cpqhpc_remove_one, */
 };
 
-
-
 static int __init cpqhpc_init(void)
 {
         int result;
@@ -1518,7 +1474,6 @@ static int __init cpqhpc_init(void)
         return result;
 }
 
-
 static void __exit cpqhpc_cleanup(void)
 {
         dbg("unload_cpqphpd()\n");
@@ -1529,8 +1484,5 @@ static void __exit cpqhpc_cleanup(void)
         cpqhp_shutdown_debugfs();
 }
 
-
 module_init(cpqhpc_init);
 module_exit(cpqhpc_cleanup);
-
-
diff --git a/drivers/pci/hotplug/cpqphp_ctrl.c b/drivers/pci/hotplug/cpqphp_ctrl.c
index cc227a8c4b11..2fa47af992a8 100644
--- a/drivers/pci/hotplug/cpqphp_ctrl.c
+++ b/drivers/pci/hotplug/cpqphp_ctrl.c
@@ -81,14 +81,15 @@ static u8 handle_switch_change(u8 change, struct controller * ctrl)
 
         for (hp_slot = 0; hp_slot < 6; hp_slot++) {
                 if (change & (0x1L << hp_slot)) {
-                        /**********************************
+                        /*
                          * this one changed.
-                         **********************************/
+                         */
                         func = cpqhp_slot_find(ctrl->bus,
                                 (hp_slot + ctrl->slot_device_offset), 0);
 
                         /* this is the structure that tells the worker thread
-                         *what to do */
+                         * what to do
+                         */
                         taskInfo = &(ctrl->event_queue[ctrl->next_event]);
                         ctrl->next_event = (ctrl->next_event + 1) % 10;
                         taskInfo->hp_slot = hp_slot;
@@ -100,17 +101,17 @@ static u8 handle_switch_change(u8 change, struct controller * ctrl)
                         func->presence_save |= (temp_word >> (hp_slot + 7)) & 0x02;
 
                         if (ctrl->ctrl_int_comp & (0x1L << hp_slot)) {
-                                /**********************************
+                                /*
                                  * Switch opened
-                                 **********************************/
+                                 */
 
                                 func->switch_save = 0;
 
                                 taskInfo->event_type = INT_SWITCH_OPEN;
                         } else {
-                                /**********************************
+                                /*
                                  * Switch closed
-                                 **********************************/
+                                 */
 
                                 func->switch_save = 0x10;
 
@@ -131,9 +132,8 @@ static struct slot *cpqhp_find_slot(struct controller *ctrl, u8 device)
 {
         struct slot *slot = ctrl->slot;
 
-        while (slot && (slot->device != device)) {
+        while (slot && (slot->device != device))
                 slot = slot->next;
-        }
 
         return slot;
 }
@@ -152,17 +152,17 @@ static u8 handle_presence_change(u16 change, struct controller * ctrl)
         if (!change)
                 return 0;
 
-        /**********************************
+        /*
          * Presence Change
-         **********************************/
+         */
         dbg("cpqsbd:  Presence/Notify input change.\n");
         dbg("         Changed bits are 0x%4.4x\n", change );
 
         for (hp_slot = 0; hp_slot < 6; hp_slot++) {
                 if (change & (0x0101 << hp_slot)) {
-                        /**********************************
+                        /*
                          * this one changed.
-                         **********************************/
+                         */
                         func = cpqhp_slot_find(ctrl->bus,
                                 (hp_slot + ctrl->slot_device_offset), 0);
 
@@ -177,22 +177,23 @@ static u8 handle_presence_change(u16 change, struct controller * ctrl)
                                 return 0;
 
                         /* If the switch closed, must be a button
-                         * If not in button mode, nevermind */
+                         * If not in button mode, nevermind
+                         */
                         if (func->switch_save && (ctrl->push_button == 1)) {
                                 temp_word = ctrl->ctrl_int_comp >> 16;
                                 temp_byte = (temp_word >> hp_slot) & 0x01;
                                 temp_byte |= (temp_word >> (hp_slot + 7)) & 0x02;
 
                                 if (temp_byte != func->presence_save) {
-                                        /**************************************
+                                        /*
                                          * button Pressed (doesn't do anything)
-                                         **************************************/
+                                         */
                                         dbg("hp_slot %d button pressed\n", hp_slot);
                                         taskInfo->event_type = INT_BUTTON_PRESS;
                                 } else {
-                                        /**********************************
+                                        /*
                                          * button Released - TAKE ACTION!!!!
-                                         **********************************/
+                                         */
                                         dbg("hp_slot %d button released\n", hp_slot);
                                         taskInfo->event_type = INT_BUTTON_RELEASE;
 
@@ -210,7 +211,8 @@ static u8 handle_presence_change(u16 change, struct controller * ctrl)
                                 }
                         } else {
                                 /* Switch is open, assume a presence change
-                                 * Save the presence state */
+                                 * Save the presence state
+                                 */
                                 temp_word = ctrl->ctrl_int_comp >> 16;
                                 func->presence_save = (temp_word >> hp_slot) & 0x01;
                                 func->presence_save |= (temp_word >> (hp_slot + 7)) & 0x02;
@@ -241,17 +243,17 @@ static u8 handle_power_fault(u8 change, struct controller * ctrl)
         if (!change)
                 return 0;
 
-        /**********************************
+        /*
          * power fault
-         **********************************/
+         */
 
         info("power fault interrupt\n");
 
         for (hp_slot = 0; hp_slot < 6; hp_slot++) {
                 if (change & (0x01 << hp_slot)) {
-                        /**********************************
+                        /*
                          * this one changed.
-                         **********************************/
+                         */
                         func = cpqhp_slot_find(ctrl->bus,
                                 (hp_slot + ctrl->slot_device_offset), 0);
 
@@ -262,16 +264,16 @@ static u8 handle_power_fault(u8 change, struct controller * ctrl)
                         rc++;
 
                         if (ctrl->ctrl_int_comp & (0x00000100 << hp_slot)) {
-                                /**********************************
+                                /*
                                  * power fault Cleared
-                                 **********************************/
+                                 */
                                 func->status = 0x00;
 
                                 taskInfo->event_type = INT_POWER_FAULT_CLEAR;
                         } else {
-                                /**********************************
+                                /*
                                  * power fault
-                                 **********************************/
+                                 */
                                 taskInfo->event_type = INT_POWER_FAULT;
 
                                 if (ctrl->rev < 4) {
@@ -432,13 +434,15 @@ static struct pci_resource *do_pre_bridge_resource_split(struct pci_resource **h
 
 
         /* If we got here, there the bridge requires some of the resource, but
-         * we may be able to split some off of the front */
+         * we may be able to split some off of the front
+         */
 
         node = *head;
 
         if (node->length & (alignment -1)) {
                 /* this one isn't an aligned length, so we'll make a new entry
-                 * and split it up. */
+                 * and split it up.
+                 */
                 split_node = kmalloc(sizeof(*split_node), GFP_KERNEL);
 
                 if (!split_node)
@@ -544,10 +548,10 @@ static struct pci_resource *get_io_resource(struct pci_resource **head, u32 size
         if (!(*head))
                 return NULL;
 
-        if ( cpqhp_resource_sort_and_combine(head) )
+        if (cpqhp_resource_sort_and_combine(head))
                 return NULL;
 
-        if ( sort_by_size(head) )
+        if (sort_by_size(head))
                 return NULL;
 
         for (node = *head; node; node = node->next) {
@@ -556,7 +560,8 @@ static struct pci_resource *get_io_resource(struct pci_resource **head, u32 size
 
                 if (node->base & (size - 1)) {
                         /* this one isn't base aligned properly
-                         * so we'll make a new entry and split it up */
+                         * so we'll make a new entry and split it up
+                         */
                         temp_dword = (node->base | (size-1)) + 1;
 
                         /* Short circuit if adjusted size is too small */
@@ -581,7 +586,8 @@ static struct pci_resource *get_io_resource(struct pci_resource **head, u32 size
                 /* Don't need to check if too small since we already did */
                 if (node->length > size) {
                         /* this one is longer than we need
-                         * so we'll make a new entry and split it up */
+                         * so we'll make a new entry and split it up
+                         */
                         split_node = kmalloc(sizeof(*split_node), GFP_KERNEL);
 
                         if (!split_node)
@@ -601,7 +607,8 @@ static struct pci_resource *get_io_resource(struct pci_resource **head, u32 size
                         continue;
 
                 /* If we got here, then it is the right size
-                 * Now take it out of the list and break */
+                 * Now take it out of the list and break
+                 */
                 if (*head == node) {
                         *head = node->next;
                 } else {
@@ -642,14 +649,16 @@ static struct pci_resource *get_max_resource(struct pci_resource **head, u32 siz
                 return NULL;
 
         for (max = *head; max; max = max->next) {
-                /* If not big enough we could probably just bail, 
-                 * instead we'll continue to the next. */
+                /* If not big enough we could probably just bail,
+                 * instead we'll continue to the next.
+                 */
                 if (max->length < size)
                         continue;
 
                 if (max->base & (size - 1)) {
                         /* this one isn't base aligned properly
-                         * so we'll make a new entry and split it up */
+                         * so we'll make a new entry and split it up
+                         */
                         temp_dword = (max->base | (size-1)) + 1;
 
                         /* Short circuit if adjusted size is too small */
@@ -672,7 +681,8 @@ static struct pci_resource *get_max_resource(struct pci_resource **head, u32 siz
 
                 if ((max->base + max->length) & (size - 1)) {
                         /* this one isn't end aligned properly at the top
-                         * so we'll make a new entry and split it up */
+                         * so we'll make a new entry and split it up
+                         */
                         split_node = kmalloc(sizeof(*split_node), GFP_KERNEL);
 
                         if (!split_node)
@@ -744,7 +754,8 @@ static struct pci_resource *get_resource(struct pci_resource **head, u32 size)
                 if (node->base & (size - 1)) {
                         dbg("%s: not aligned\n", __func__);
                         /* this one isn't base aligned properly
-                         * so we'll make a new entry and split it up */
+                         * so we'll make a new entry and split it up
+                         */
                         temp_dword = (node->base | (size-1)) + 1;
 
                         /* Short circuit if adjusted size is too small */
@@ -769,7 +780,8 @@ static struct pci_resource *get_resource(struct pci_resource **head, u32 size)
                 if (node->length > size) {
                         dbg("%s: too big\n", __func__);
                         /* this one is longer than we need
-                         * so we'll make a new entry and split it up */
+                         * so we'll make a new entry and split it up
+                         */
                         split_node = kmalloc(sizeof(*split_node), GFP_KERNEL);
 
                         if (!split_node)
@@ -886,19 +898,19 @@ irqreturn_t cpqhp_ctrl_intr(int IRQ, void *data)
         u32 Diff;
         u32 temp_dword;
 
-        
+
         misc = readw(ctrl->hpc_reg + MISC);
-        /***************************************
+        /*
          * Check to see if it was our interrupt
-         ***************************************/
+         */
         if (!(misc & 0x000C)) {
                 return IRQ_NONE;
         }
 
         if (misc & 0x0004) {
-                /**********************************
+                /*
                  * Serial Output interrupt Pending
-                 **********************************/
+                 */
 
                 /* Clear the interrupt */
                 misc |= 0x0004;
@@ -961,11 +973,8 @@ struct pci_func *cpqhp_slot_create(u8 busnumber)
         struct pci_func *next;
 
         new_slot = kzalloc(sizeof(*new_slot), GFP_KERNEL);
-        if (new_slot == NULL) {
-                /* I'm not dead yet!
-                 * You will be. */
+        if (new_slot == NULL)
                 return new_slot;
-        }
 
         new_slot->next = NULL;
         new_slot->configured = 1;
@@ -996,10 +1005,8 @@ static int slot_remove(struct pci_func * old_slot)
                 return 1;
 
         next = cpqhp_slot_list[old_slot->bus];
-
-        if (next == NULL) {
+        if (next == NULL)
                 return 1;
-        }
 
         if (next == old_slot) {
                 cpqhp_slot_list[old_slot->bus] = old_slot->next;
@@ -1008,9 +1015,8 @@ static int slot_remove(struct pci_func * old_slot)
                 return 0;
         }
 
-        while ((next->next != old_slot) && (next->next != NULL)) {
+        while ((next->next != old_slot) && (next->next != NULL))
                 next = next->next;
-        }
 
         if (next->next == old_slot) {
                 next->next = old_slot->next;
@@ -1040,9 +1046,8 @@ static int bridge_slot_remove(struct pci_func *bridge)
         for (tempBus = secondaryBus; tempBus <= subordinateBus; tempBus++) {
                 next = cpqhp_slot_list[tempBus];
 
-                while (!slot_remove(next)) {
+                while (!slot_remove(next))
                         next = cpqhp_slot_list[tempBus];
-                }
         }
 
         next = cpqhp_slot_list[bridge->bus];
@@ -1130,39 +1135,43 @@ static u8 set_controller_speed(struct controller *ctrl, u8 adapter_speed, u8 hp_
         u8 slot_power = readb(ctrl->hpc_reg + SLOT_POWER);
         u16 reg16;
         u32 leds = readl(ctrl->hpc_reg + LED_CONTROL);
-        
+
         if (ctrl->speed == adapter_speed)
                 return 0;
-        
+
         /* We don't allow freq/mode changes if we find another adapter running
-         * in another slot on this controller */
+         * in another slot on this controller
+         */
         for(slot = ctrl->slot; slot; slot = slot->next) {
-                if (slot->device == (hp_slot + ctrl->slot_device_offset)) 
+                if (slot->device == (hp_slot + ctrl->slot_device_offset))
                         continue;
                 if (!slot->hotplug_slot || !slot->hotplug_slot->info)
                         continue;
-                if (slot->hotplug_slot->info->adapter_status == 0) 
+                if (slot->hotplug_slot->info->adapter_status == 0)
                         continue;
                 /* If another adapter is running on the same segment but at a
                  * lower speed/mode, we allow the new adapter to function at
-                 * this rate if supported */
-                if (ctrl->speed < adapter_speed) 
+                 * this rate if supported
+                 */
+                if (ctrl->speed < adapter_speed)
                         return 0;
 
                 return 1;
         }
-        
+
         /* If the controller doesn't support freq/mode changes and the
-         * controller is running at a higher mode, we bail */
+         * controller is running at a higher mode, we bail
+         */
         if ((ctrl->speed > adapter_speed) && (!ctrl->pcix_speed_capability))
                 return 1;
-        
+
         /* But we allow the adapter to run at a lower rate if possible */
         if ((ctrl->speed < adapter_speed) && (!ctrl->pcix_speed_capability))
                 return 0;
 
         /* We try to set the max speed supported by both the adapter and
-         * controller */
+         * controller
+         */
         if (ctrl->speed_capability < adapter_speed) {
                 if (ctrl->speed == ctrl->speed_capability)
                         return 0;
@@ -1171,22 +1180,22 @@ static u8 set_controller_speed(struct controller *ctrl, u8 adapter_speed, u8 hp_
 
         writel(0x0L, ctrl->hpc_reg + LED_CONTROL);
         writeb(0x00, ctrl->hpc_reg + SLOT_ENABLE);
-        
-        set_SOGO(ctrl); 
+
+        set_SOGO(ctrl);
         wait_for_ctrl_irq(ctrl);
-        
+
         if (adapter_speed != PCI_SPEED_133MHz_PCIX)
                 reg = 0xF5;
         else
-                reg = 0xF4;     
+                reg = 0xF4;
         pci_write_config_byte(ctrl->pci_dev, 0x41, reg);
-        
+
         reg16 = readw(ctrl->hpc_reg + NEXT_CURR_FREQ);
         reg16 &= ~0x000F;
         switch(adapter_speed) {
-                case(PCI_SPEED_133MHz_PCIX): 
+                case(PCI_SPEED_133MHz_PCIX):
                         reg = 0x75;
-                        reg16 |= 0xB; 
+                        reg16 |= 0xB;
                         break;
                 case(PCI_SPEED_100MHz_PCIX):
                         reg = 0x74;
@@ -1203,48 +1212,48 @@ static u8 set_controller_speed(struct controller *ctrl, u8 adapter_speed, u8 hp_
                 default: /* 33MHz PCI 2.2 */
                         reg = 0x71;
                         break;
-                        
+
         }
         reg16 |= 0xB << 12;
         writew(reg16, ctrl->hpc_reg + NEXT_CURR_FREQ);
-        
-        mdelay(5); 
-        
+
+        mdelay(5);
+
         /* Reenable interrupts */
         writel(0, ctrl->hpc_reg + INT_MASK);
 
-        pci_write_config_byte(ctrl->pci_dev, 0x41, reg); 
-        
+        pci_write_config_byte(ctrl->pci_dev, 0x41, reg);
+
         /* Restart state machine */
         reg = ~0xF;
         pci_read_config_byte(ctrl->pci_dev, 0x43, &reg);
         pci_write_config_byte(ctrl->pci_dev, 0x43, reg);
-        
+
         /* Only if mode change...*/
         if (((ctrl->speed == PCI_SPEED_66MHz) && (adapter_speed == PCI_SPEED_66MHz_PCIX)) ||
                 ((ctrl->speed == PCI_SPEED_66MHz_PCIX) && (adapter_speed == PCI_SPEED_66MHz))) 
                         set_SOGO(ctrl);
-        
+
         wait_for_ctrl_irq(ctrl);
         mdelay(1100);
-        
+
         /* Restore LED/Slot state */
         writel(leds, ctrl->hpc_reg + LED_CONTROL);
         writeb(slot_power, ctrl->hpc_reg + SLOT_ENABLE);
-        
+
         set_SOGO(ctrl);
         wait_for_ctrl_irq(ctrl);
 
         ctrl->speed = adapter_speed;
         slot = cpqhp_find_slot(ctrl, hp_slot + ctrl->slot_device_offset);
 
-        info("Successfully changed frequency/mode for adapter in slot %d\n", 
+        info("Successfully changed frequency/mode for adapter in slot %d\n",
                         slot->number);
         return 0;
 }
 
-/* the following routines constitute the bulk of the 
-   hotplug controller logic
+/* the following routines constitute the bulk of the
+ * hotplug controller logic
  */
 
 
@@ -1268,17 +1277,17 @@ static u32 board_replaced(struct pci_func *func, struct controller *ctrl)
 
         hp_slot = func->device - ctrl->slot_device_offset;
 
-        if (readl(ctrl->hpc_reg + INT_INPUT_CLEAR) & (0x01L << hp_slot)) {
-                /**********************************
-                 * The switch is open.
-                 **********************************/
+        /*
+         * The switch is open.
+         */
+        if (readl(ctrl->hpc_reg + INT_INPUT_CLEAR) & (0x01L << hp_slot))
                 rc = INTERLOCK_OPEN;
-        } else if (is_slot_enabled (ctrl, hp_slot)) {
-                /**********************************
-                 * The board is already on
-                 **********************************/
+        /*
+         * The board is already on
+         */
+        else if (is_slot_enabled (ctrl, hp_slot))
                 rc = CARD_FUNCTIONING;
-        } else {
+        else {
                 mutex_lock(&ctrl->crit_sect);
 
                 /* turn on board without attaching to the bus */
@@ -1299,7 +1308,7 @@ static u32 board_replaced(struct pci_func *func, struct controller *ctrl)
 
                 /* Wait for SOBS to be unset */
                 wait_for_ctrl_irq (ctrl);
-                
+
                 adapter_speed = get_adapter_speed(ctrl, hp_slot);
                 if (ctrl->speed != adapter_speed)
                         if (set_controller_speed(ctrl, adapter_speed, hp_slot))
@@ -1352,7 +1361,8 @@ static u32 board_replaced(struct pci_func *func, struct controller *ctrl)
                          * Get slot won't work for devices behind
                          * bridges, but in this case it will always be
                          * called for the "base" bus/dev/func of an
-                         * adapter. */
+                         * adapter.
+                         */
 
                         mutex_lock(&ctrl->crit_sect);
 
@@ -1377,7 +1387,8 @@ static u32 board_replaced(struct pci_func *func, struct controller *ctrl)
 
                          * Get slot won't work for devices behind bridges, but
                          * in this case it will always be called for the "base"
-                         * bus/dev/func of an adapter. */
+                         * bus/dev/func of an adapter.
+                         */
 
                         mutex_lock(&ctrl->crit_sect);
 
@@ -1434,7 +1445,8 @@ static u32 board_added(struct pci_func *func, struct controller *ctrl)
         wait_for_ctrl_irq (ctrl);
 
         /* Change bits in slot power register to force another shift out
-         * NOTE: this is to work around the timer bug */
+         * NOTE: this is to work around the timer bug
+         */
         temp_byte = readb(ctrl->hpc_reg + SLOT_POWER);
         writeb(0x00, ctrl->hpc_reg + SLOT_POWER);
         writeb(temp_byte, ctrl->hpc_reg + SLOT_POWER);
@@ -1443,12 +1455,12 @@ static u32 board_added(struct pci_func *func, struct controller *ctrl)
 
         /* Wait for SOBS to be unset */
         wait_for_ctrl_irq (ctrl);
-        
+
         adapter_speed = get_adapter_speed(ctrl, hp_slot);
         if (ctrl->speed != adapter_speed)
                 if (set_controller_speed(ctrl, adapter_speed, hp_slot))
                         rc = WRONG_BUS_FREQUENCY;
-        
+
         /* turn off board without attaching to the bus */
         disable_slot_power (ctrl, hp_slot);
 
@@ -1461,7 +1473,7 @@ static u32 board_added(struct pci_func *func, struct controller *ctrl)
 
         if (rc)
                 return rc;
-        
+
         p_slot = cpqhp_find_slot(ctrl, hp_slot + ctrl->slot_device_offset);
 
         /* turn on board and blink green LED */
@@ -1521,7 +1533,7 @@ static u32 board_added(struct pci_func *func, struct controller *ctrl)
         }
 
         /* All F's is an empty slot or an invalid board */
-        if (temp_register != 0xFFFFFFFF) {        /* Check for a board in the slot */
+        if (temp_register != 0xFFFFFFFF) {
                 res_lists.io_head = ctrl->io_head;
                 res_lists.mem_head = ctrl->mem_head;
                 res_lists.p_mem_head = ctrl->p_mem_head;
@@ -1570,9 +1582,8 @@ static u32 board_added(struct pci_func *func, struct controller *ctrl)
                 index = 0;
                 do {
                         new_slot = cpqhp_slot_find(ctrl->bus, func->device, index++);
-                        if (new_slot && !new_slot->pci_dev) {
+                        if (new_slot && !new_slot->pci_dev)
                                 cpqhp_configure_device(ctrl, new_slot);
-                        }
                 } while (new_slot);
 
                 mutex_lock(&ctrl->crit_sect);
@@ -1859,12 +1870,12 @@ static void interrupt_event_handler(struct controller *ctrl)
                                                 info(msg_button_on, p_slot->number);
                                         }
                                         mutex_lock(&ctrl->crit_sect);
-                                        
+
                                         dbg("blink green LED and turn off amber\n");
-                                        
+
                                         amber_LED_off (ctrl, hp_slot);
                                         green_LED_blink (ctrl, hp_slot);
-                                        
+
                                         set_SOGO(ctrl);
 
                                         /* Wait for SOBS to be unset */
@@ -1958,7 +1969,7 @@ void cpqhp_pushbutton_thread(unsigned long slot)
                         if (cpqhp_process_SI(ctrl, func) != 0) {
                                 amber_LED_on(ctrl, hp_slot);
                                 green_LED_off(ctrl, hp_slot);
-                                
+
                                 set_SOGO(ctrl);
 
                                 /* Wait for SOBS to be unset */
@@ -2079,7 +2090,7 @@ int cpqhp_process_SS(struct controller *ctrl, struct pci_func *func)
         struct pci_bus *pci_bus = ctrl->pci_bus;
         int physical_slot=0;
 
-        device = func->device; 
+        device = func->device;
         func = cpqhp_slot_find(ctrl->bus, device, index++);
         p_slot = cpqhp_find_slot(ctrl, device);
         if (p_slot) {
@@ -2113,9 +2124,8 @@ int cpqhp_process_SS(struct controller *ctrl, struct pci_func *func)
 
                                 /* If the VGA Enable bit is set, remove isn't
                                  * supported */
-                                if (BCR & PCI_BRIDGE_CTL_VGA) {
+                                if (BCR & PCI_BRIDGE_CTL_VGA)
                                         rc = REMOVE_NOT_SUPPORTED;
-                                }
                         }
                 }
 
@@ -2183,67 +2193,67 @@ int cpqhp_hardware_test(struct controller *ctrl, int test_num)
         num_of_slots = readb(ctrl->hpc_reg + SLOT_MASK) & 0x0f;
 
         switch (test_num) {
-                case 1:
-                        /* Do stuff here! */
-
-                        /* Do that funky LED thing */
-                        /* so we can restore them later */
-                        save_LED = readl(ctrl->hpc_reg + LED_CONTROL);
-                        work_LED = 0x01010101;
-                        switch_leds(ctrl, num_of_slots, &work_LED, 0);
-                        switch_leds(ctrl, num_of_slots, &work_LED, 1);
-                        switch_leds(ctrl, num_of_slots, &work_LED, 0);
-                        switch_leds(ctrl, num_of_slots, &work_LED, 1);
-
-                        work_LED = 0x01010000;
-                        writel(work_LED, ctrl->hpc_reg + LED_CONTROL);
-                        switch_leds(ctrl, num_of_slots, &work_LED, 0);
-                        switch_leds(ctrl, num_of_slots, &work_LED, 1);
-                        work_LED = 0x00000101;
-                        writel(work_LED, ctrl->hpc_reg + LED_CONTROL);
-                        switch_leds(ctrl, num_of_slots, &work_LED, 0);
-                        switch_leds(ctrl, num_of_slots, &work_LED, 1);
+        case 1:
+                /* Do stuff here! */
+
+                /* Do that funky LED thing */
+                /* so we can restore them later */
+                save_LED = readl(ctrl->hpc_reg + LED_CONTROL);
+                work_LED = 0x01010101;
+                switch_leds(ctrl, num_of_slots, &work_LED, 0);
+                switch_leds(ctrl, num_of_slots, &work_LED, 1);
+                switch_leds(ctrl, num_of_slots, &work_LED, 0);
+                switch_leds(ctrl, num_of_slots, &work_LED, 1);
+
+                work_LED = 0x01010000;
+                writel(work_LED, ctrl->hpc_reg + LED_CONTROL);
+                switch_leds(ctrl, num_of_slots, &work_LED, 0);
+                switch_leds(ctrl, num_of_slots, &work_LED, 1);
+                work_LED = 0x00000101;
+                writel(work_LED, ctrl->hpc_reg + LED_CONTROL);
+                switch_leds(ctrl, num_of_slots, &work_LED, 0);
+                switch_leds(ctrl, num_of_slots, &work_LED, 1);
+
+                work_LED = 0x01010000;
+                writel(work_LED, ctrl->hpc_reg + LED_CONTROL);
+                for (loop = 0; loop < num_of_slots; loop++) {
+                        set_SOGO(ctrl);
 
-                        work_LED = 0x01010000;
-                        writel(work_LED, ctrl->hpc_reg + LED_CONTROL);
-                        for (loop = 0; loop < num_of_slots; loop++) {
-                                set_SOGO(ctrl);
+                        /* Wait for SOGO interrupt */
+                        wait_for_ctrl_irq (ctrl);
 
-                                /* Wait for SOGO interrupt */
-                                wait_for_ctrl_irq (ctrl);
+                        /* Get ready for next iteration */
+                        long_delay((3*HZ)/10);
+                        work_LED = work_LED >> 16;
+                        writel(work_LED, ctrl->hpc_reg + LED_CONTROL);
 
-                                /* Get ready for next iteration */
-                                long_delay((3*HZ)/10);
-                                work_LED = work_LED >> 16;
-                                writel(work_LED, ctrl->hpc_reg + LED_CONTROL);
-                                
-                                set_SOGO(ctrl);
+                        set_SOGO(ctrl);
 
-                                /* Wait for SOGO interrupt */
-                                wait_for_ctrl_irq (ctrl);
+                        /* Wait for SOGO interrupt */
+                        wait_for_ctrl_irq (ctrl);
 
-                                /* Get ready for next iteration */
-                                long_delay((3*HZ)/10);
-                                work_LED = work_LED << 16;
-                                writel(work_LED, ctrl->hpc_reg + LED_CONTROL);
-                                work_LED = work_LED << 1;
-                                writel(work_LED, ctrl->hpc_reg + LED_CONTROL);
-                        }
+                        /* Get ready for next iteration */
+                        long_delay((3*HZ)/10);
+                        work_LED = work_LED << 16;
+                        writel(work_LED, ctrl->hpc_reg + LED_CONTROL);
+                        work_LED = work_LED << 1;
+                        writel(work_LED, ctrl->hpc_reg + LED_CONTROL);
+                }
 
-                        /* put it back the way it was */
-                        writel(save_LED, ctrl->hpc_reg + LED_CONTROL);
+                /* put it back the way it was */
+                writel(save_LED, ctrl->hpc_reg + LED_CONTROL);
 
-                        set_SOGO(ctrl);
+                set_SOGO(ctrl);
 
-                        /* Wait for SOBS to be unset */
-                        wait_for_ctrl_irq (ctrl);
-                        break;
-                case 2:
-                        /* Do other stuff here! */
-                        break;
-                case 3:
-                        /* and more... */
-                        break;
+                /* Wait for SOBS to be unset */
+                wait_for_ctrl_irq (ctrl);
+                break;
+        case 2:
+                /* Do other stuff here! */
+                break;
+        case 3:
+                /* and more... */
+                break;
         }
         return 0;
 }
@@ -2312,9 +2322,9 @@ static u32 configure_new_device(struct controller * ctrl, struct pci_func * func
                 while ((function < max_functions) && (!stop_it)) {
                         pci_bus_read_config_dword (ctrl->pci_bus, PCI_DEVFN(func->device, function), 0x00, &ID);
 
-                        if (ID == 0xFFFFFFFF) {   /* There's nothing there. */
+                        if (ID == 0xFFFFFFFF) {
                                 function++;
-                        } else {  /* There's something there */
+                        } else {
                                 /* Setup slot structure. */
                                 new_slot = cpqhp_slot_create(func->bus);
 
@@ -2339,8 +2349,8 @@ static u32 configure_new_device(struct controller * ctrl, struct pci_func * func
 
 
 /*
-  Configuration logic that involves the hotplug data structures and 
-  their bookkeeping
+ * Configuration logic that involves the hotplug data structures and
+ * their bookkeeping
  */
 
 
@@ -2393,7 +2403,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
         if (rc)
                 return rc;
 
-        if ((temp_byte & 0x7F) == PCI_HEADER_TYPE_BRIDGE) { /* PCI-PCI Bridge */
+        if ((temp_byte & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
                 /* set Primary bus */
                 dbg("set Primary bus = %d\n", func->bus);
                 rc = pci_bus_write_config_byte(pci_bus, devfn, PCI_PRIMARY_BUS, func->bus);
@@ -2484,7 +2494,8 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
                 temp_resources.irqs = &irqs;
 
                 /* Make copies of the nodes we are going to pass down so that
-                 * if there is a problem,we can just use these to free resources */
+                 * if there is a problem,we can just use these to free resources
+                 */
                 hold_bus_node = kmalloc(sizeof(*hold_bus_node), GFP_KERNEL);
                 hold_IO_node = kmalloc(sizeof(*hold_IO_node), GFP_KERNEL);
                 hold_mem_node = kmalloc(sizeof(*hold_mem_node), GFP_KERNEL);
@@ -2556,7 +2567,8 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
                 temp_word = (p_mem_node->base + p_mem_node->length - 1) >> 16;
                 rc = pci_bus_write_config_word (pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, temp_word);
 
-                /* Adjust this to compensate for extra adjustment in first loop */
+                /* Adjust this to compensate for extra adjustment in first loop
+                 */
                 irqs.barber_pole--;
 
                 rc = 0;
@@ -2917,27 +2929,26 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
                 }               /* End of base register loop */
                 if (cpqhp_legacy_mode) {
                         /* Figure out which interrupt pin this function uses */
-                        rc = pci_bus_read_config_byte (pci_bus, devfn, 
+                        rc = pci_bus_read_config_byte (pci_bus, devfn,
                                 PCI_INTERRUPT_PIN, &temp_byte);
 
                         /* If this function needs an interrupt and we are behind
                          * a bridge and the pin is tied to something that's
                          * alread mapped, set this one the same */
-                        if (temp_byte && resources->irqs && 
-                            (resources->irqs->valid_INT & 
+                        if (temp_byte && resources->irqs &&
+                            (resources->irqs->valid_INT &
                              (0x01 << ((temp_byte + resources->irqs->barber_pole - 1) & 0x03)))) {
                                 /* We have to share with something already set up */
-                                IRQ = resources->irqs->interrupt[(temp_byte + 
+                                IRQ = resources->irqs->interrupt[(temp_byte +
                                         resources->irqs->barber_pole - 1) & 0x03];
                         } else {
                                 /* Program IRQ based on card type */
                                 rc = pci_bus_read_config_byte (pci_bus, devfn, 0x0B, &class_code);
 
-                                if (class_code == PCI_BASE_CLASS_STORAGE) {
+                                if (class_code == PCI_BASE_CLASS_STORAGE)
                                         IRQ = cpqhp_disk_irq;
-                                } else {
+                                else
                                         IRQ = cpqhp_nic_irq;
-                                }
                         }
 
                         /* IRQ Line */
diff --git a/drivers/pci/hotplug/cpqphp_nvram.c b/drivers/pci/hotplug/cpqphp_nvram.c
index cb174888002b..76ba8a1c774d 100644
--- a/drivers/pci/hotplug/cpqphp_nvram.c
+++ b/drivers/pci/hotplug/cpqphp_nvram.c
@@ -94,12 +94,13 @@ static u8 evbuffer[1024];
 
 static void __iomem *compaq_int15_entry_point;
 
-static spinlock_t int15_lock;           /* lock for ordering int15_bios_call() */
+/* lock for ordering int15_bios_call() */
+static spinlock_t int15_lock;
 
 
 /* This is a series of function that deals with
-   setting & getting the hotplug resource table in some environment variable.
-*/
+ * setting & getting the hotplug resource table in some environment variable.
+ */
 
 /*
  * We really shouldn't be doing this unless there is a _very_ good reason to!!!
@@ -113,7 +114,7 @@ static u32 add_byte( u32 **p_buffer, u8 value, u32 *used, u32 *avail)
 
         if ((*used + 1) > *avail)
                 return(1);
-        
+
         *((u8*)*p_buffer) = value;
         tByte = (u8**)p_buffer;
         (*tByte)++;
@@ -170,10 +171,10 @@ static u32 access_EV (u16 operation, u8 *ev_name, u8 *buffer, u32 *buf_size)
         unsigned long flags;
         int op = operation;
         int ret_val;
-        
+
         if (!compaq_int15_entry_point)
                 return -ENODEV;
-        
+
         spin_lock_irqsave(&int15_lock, flags);
         __asm__ (
                 "xorl   %%ebx,%%ebx\n" \
@@ -187,7 +188,7 @@ static u32 access_EV (u16 operation, u8 *ev_name, u8 *buffer, u32 *buf_size)
                 "D" (buffer), "m" (compaq_int15_entry_point)
                 : "%ebx", "%edx");
         spin_unlock_irqrestore(&int15_lock, flags);
-        
+
         return((ret_val & 0xFF00) >> 8);
 }
 
@@ -210,14 +211,16 @@ static int load_HRT (void __iomem *rom_start)
 
         available = 1024;
 
-        // Now load the EV
+        /* Now load the EV */
         temp_dword = available;
 
         rc = access_EV(READ_EV, "CQTHPS", evbuffer, &temp_dword);
 
         evbuffer_length = temp_dword;
 
-        // We're maintaining the resource lists so write FF to invalidate old info
+        /* We're maintaining the resource lists so write FF to invalidate old
+         * info
+         */
         temp_dword = 1;
 
         rc = access_EV(WRITE_EV, "CQTHPS", &temp_byte, &temp_dword);
@@ -263,13 +266,13 @@ static u32 store_HRT (void __iomem *rom_start)
         p_EV_header = (struct ev_hrt_header *) pFill;
 
         ctrl = cpqhp_ctrl_list;
-        
-        // The revision of this structure
+
+        /* The revision of this structure */
         rc = add_byte( &pFill, 1 + ctrl->push_flag, &usedbytes, &available);
         if (rc)
                 return(rc);
 
-        // The number of controllers
+        /* The number of controllers */
         rc = add_byte( &pFill, 1, &usedbytes, &available);
         if (rc)
                 return(rc);
@@ -279,27 +282,27 @@ static u32 store_HRT (void __iomem *rom_start)
 
                 numCtrl++;
 
-                // The bus number
+                /* The bus number */
                 rc = add_byte( &pFill, ctrl->bus, &usedbytes, &available);
                 if (rc)
                         return(rc);
 
-                // The device Number
+                /* The device Number */
                 rc = add_byte( &pFill, PCI_SLOT(ctrl->pci_dev->devfn), &usedbytes, &available);
                 if (rc)
                         return(rc);
 
-                // The function Number
+                /* The function Number */
                 rc = add_byte( &pFill, PCI_FUNC(ctrl->pci_dev->devfn), &usedbytes, &available);
                 if (rc)
                         return(rc);
 
-                // Skip the number of available entries
+                /* Skip the number of available entries */
                 rc = add_dword( &pFill, 0, &usedbytes, &available);
                 if (rc)
                         return(rc);
 
-                // Figure out memory Available
+                /* Figure out memory Available */
 
                 resNode = ctrl->mem_head;
 
@@ -308,12 +311,12 @@ static u32 store_HRT (void __iomem *rom_start)
                 while (resNode) {
                         loop ++;
 
-                        // base
+                        /* base */
                         rc = add_dword( &pFill, resNode->base, &usedbytes, &available);
                         if (rc)
                                 return(rc);
 
-                        // length
+                        /* length */
                         rc = add_dword( &pFill, resNode->length, &usedbytes, &available);
                         if (rc)
                                 return(rc);
@@ -321,10 +324,10 @@ static u32 store_HRT (void __iomem *rom_start)
                         resNode = resNode->next;
                 }
 
-                // Fill in the number of entries
+                /* Fill in the number of entries */
                 p_ev_ctrl->mem_avail = loop;
 
-                // Figure out prefetchable memory Available
+                /* Figure out prefetchable memory Available */
 
                 resNode = ctrl->p_mem_head;
 
@@ -333,12 +336,12 @@ static u32 store_HRT (void __iomem *rom_start)
                 while (resNode) {
                         loop ++;
 
-                        // base
+                        /* base */
                         rc = add_dword( &pFill, resNode->base, &usedbytes, &available);
                         if (rc)
                                 return(rc);
 
-                        // length
+                        /* length */
                         rc = add_dword( &pFill, resNode->length, &usedbytes, &available);
                         if (rc)
                                 return(rc);
@@ -346,10 +349,10 @@ static u32 store_HRT (void __iomem *rom_start)
                         resNode = resNode->next;
                 }
 
-                // Fill in the number of entries
+                /* Fill in the number of entries */
                 p_ev_ctrl->p_mem_avail = loop;
 
-                // Figure out IO Available
+                /* Figure out IO Available */
 
                 resNode = ctrl->io_head;
 
@@ -358,12 +361,12 @@ static u32 store_HRT (void __iomem *rom_start)
                 while (resNode) {
                         loop ++;
 
-                        // base
+                        /* base */
                         rc = add_dword( &pFill, resNode->base, &usedbytes, &available);
                         if (rc)
                                 return(rc);
 
-                        // length
+                        /* length */
                         rc = add_dword( &pFill, resNode->length, &usedbytes, &available);
                         if (rc)
                                 return(rc);
@@ -371,10 +374,10 @@ static u32 store_HRT (void __iomem *rom_start)
                         resNode = resNode->next;
                 }
 
-                // Fill in the number of entries
+                /* Fill in the number of entries */
                 p_ev_ctrl->io_avail = loop;
 
-                // Figure out bus Available
+                /* Figure out bus Available */
 
                 resNode = ctrl->bus_head;
 
@@ -383,12 +386,12 @@ static u32 store_HRT (void __iomem *rom_start)
                 while (resNode) {
                         loop ++;
 
-                        // base
+                        /* base */
                         rc = add_dword( &pFill, resNode->base, &usedbytes, &available);
                         if (rc)
                                 return(rc);
 
-                        // length
+                        /* length */
                         rc = add_dword( &pFill, resNode->length, &usedbytes, &available);
                         if (rc)
                                 return(rc);
@@ -396,15 +399,15 @@ static u32 store_HRT (void __iomem *rom_start)
                         resNode = resNode->next;
                 }
 
-                // Fill in the number of entries
+                /* Fill in the number of entries */
                 p_ev_ctrl->bus_avail = loop;
 
                 ctrl = ctrl->next;
         }
-        
+
         p_EV_header->num_of_ctrl = numCtrl;
 
-        // Now store the EV
+        /* Now store the EV */
 
         temp_dword = usedbytes;
 
@@ -449,20 +452,21 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
         struct ev_hrt_header *p_EV_header;
 
         if (!evbuffer_init) {
-                // Read the resource list information in from NVRAM
+                /* Read the resource list information in from NVRAM */
                 if (load_HRT(rom_start))
                         memset (evbuffer, 0, 1024);
 
                 evbuffer_init = 1;
         }
 
-        // If we saved information in NVRAM, use it now
+        /* If we saved information in NVRAM, use it now */
         p_EV_header = (struct ev_hrt_header *) evbuffer;
 
-        // The following code is for systems where version 1.0 of this
-        // driver has been loaded, but doesn't support the hardware.
-        // In that case, the driver would incorrectly store something
-        // in NVRAM.
+        /* The following code is for systems where version 1.0 of this
+         * driver has been loaded, but doesn't support the hardware.
+         * In that case, the driver would incorrectly store something
+         * in NVRAM.
+         */
         if ((p_EV_header->Version == 2) ||
             ((p_EV_header->Version == 1) && !ctrl->push_flag)) {
                 p_byte = &(p_EV_header->next);
@@ -479,7 +483,7 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
                 function = p_ev_ctrl->function;
 
                 while ((bus != ctrl->bus) ||
-                       (device != PCI_SLOT(ctrl->pci_dev->devfn)) || 
+                       (device != PCI_SLOT(ctrl->pci_dev->devfn)) ||
                        (function != PCI_FUNC(ctrl->pci_dev->devfn))) {
                         nummem = p_ev_ctrl->mem_avail;
                         numpmem = p_ev_ctrl->p_mem_avail;
@@ -491,7 +495,7 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
                         if (p_byte > ((u8*)p_EV_header + evbuffer_length))
                                 return 2;
 
-                        // Skip forward to the next entry
+                        /* Skip forward to the next entry */
                         p_byte += (nummem + numpmem + numio + numbus) * 8;
 
                         if (p_byte > ((u8*)p_EV_header + evbuffer_length))
@@ -629,8 +633,9 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
                         ctrl->bus_head = bus_node;
                 }
 
-                // If all of the following fail, we don't have any resources for
-                // hot plug add
+                /* If all of the following fail, we don't have any resources for
+                 * hot plug add
+                 */
                 rc = 1;
                 rc &= cpqhp_resource_sort_and_combine(&(ctrl->mem_head));
                 rc &= cpqhp_resource_sort_and_combine(&(ctrl->p_mem_head));
@@ -640,14 +645,14 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
                 if (rc)
                         return(rc);
         } else {
-                if ((evbuffer[0] != 0) && (!ctrl->push_flag)) 
+                if ((evbuffer[0] != 0) && (!ctrl->push_flag))
                         return 1;
         }
 
         return 0;
 }
 
-        
+
 int compaq_nvram_store (void __iomem *rom_start)
 {
         int rc = 1;
diff --git a/drivers/pci/hotplug/cpqphp_pci.c b/drivers/pci/hotplug/cpqphp_pci.c
index 6c0ed0fcb8ee..6173b9a4544e 100644
--- a/drivers/pci/hotplug/cpqphp_pci.c
+++ b/drivers/pci/hotplug/cpqphp_pci.c
@@ -37,7 +37,6 @@
 #include "../pci.h"
 #include "cpqphp.h"
 #include "cpqphp_nvram.h"
-#include <asm/pci_x86.h>
 
 
 u8 cpqhp_nic_irq;
@@ -82,14 +81,14 @@ static void __iomem *detect_HRT_floating_pointer(void __iomem *begin, void __iom
 }
 
 
-int cpqhp_configure_device (struct controller* ctrl, struct pci_func* func)  
+int cpqhp_configure_device (struct controller* ctrl, struct pci_func* func)
 {
         unsigned char bus;
         struct pci_bus *child;
         int num;
 
         if (func->pci_dev == NULL)
-                func->pci_dev = pci_find_slot(func->bus, PCI_DEVFN(func->device, func->function));
+                func->pci_dev = pci_get_bus_and_slot(func->bus,PCI_DEVFN(func->device, func->function));
 
         /* No pci device, we need to create it then */
         if (func->pci_dev == NULL) {
@@ -99,7 +98,7 @@ int cpqhp_configure_device (struct controller* ctrl, struct pci_func* func)
                 if (num)
                         pci_bus_add_devices(ctrl->pci_dev->bus);
 
-                func->pci_dev = pci_find_slot(func->bus, PCI_DEVFN(func->device, func->function));
+                func->pci_dev = pci_get_bus_and_slot(func->bus, PCI_DEVFN(func->device, func->function));
                 if (func->pci_dev == NULL) {
                         dbg("ERROR: pci_dev still null\n");
                         return 0;
@@ -112,20 +111,24 @@ int cpqhp_configure_device (struct controller* ctrl, struct pci_func* func)
                 pci_do_scan_bus(child);
         }
 
+        pci_dev_put(func->pci_dev);
+
         return 0;
 }
 
 
-int cpqhp_unconfigure_device(struct pci_func* func) 
+int cpqhp_unconfigure_device(struct pci_func* func)
 {
         int j;
-        
+
         dbg("%s: bus/dev/func = %x/%x/%x\n", __func__, func->bus, func->device, func->function);
 
         for (j=0; j<8 ; j++) {
-                struct pci_dev* temp = pci_find_slot(func->bus, PCI_DEVFN(func->device, j));
-                if (temp)
+                struct pci_dev* temp = pci_get_bus_and_slot(func->bus, PCI_DEVFN(func->device, j));
+                if (temp) {
+                        pci_dev_put(temp);
                         pci_remove_bus_device(temp);
+                }
         }
         return 0;
 }
@@ -178,32 +181,22 @@ int cpqhp_set_irq (u8 bus_num, u8 dev_num, u8 int_pin, u8 irq_num)
                 if (!rc)
                         return !rc;
 
-                // set the Edge Level Control Register (ELCR)
+                /* set the Edge Level Control Register (ELCR) */
                 temp_word = inb(0x4d0);
                 temp_word |= inb(0x4d1) << 8;
 
                 temp_word |= 0x01 << irq_num;
 
-                // This should only be for x86 as it sets the Edge Level Control Register
-                outb((u8) (temp_word & 0xFF), 0x4d0);
-                outb((u8) ((temp_word & 0xFF00) >> 8), 0x4d1);
-                rc = 0;
-        }
+                /* This should only be for x86 as it sets the Edge Level
+                 * Control Register
+                 */
+                outb((u8) (temp_word & 0xFF), 0x4d0); outb((u8) ((temp_word &
+                0xFF00) >> 8), 0x4d1); rc = 0; }
 
         return rc;
 }
 
 
-/*
- * WTF??? This function isn't in the code, yet a function calls it, but the 
- * compiler optimizes it away?  strange.  Here as a placeholder to keep the 
- * compiler happy.
- */
-static int PCI_ScanBusNonBridge (u8 bus, u8 device)
-{
-        return 0;
-}
-
 static int PCI_ScanBusForNonBridge(struct controller *ctrl, u8 bus_num, u8 * dev_num)
 {
         u16 tdevice;
@@ -213,11 +206,11 @@ static int PCI_ScanBusForNonBridge(struct controller *ctrl, u8 bus_num, u8 * dev
         ctrl->pci_bus->number = bus_num;
 
         for (tdevice = 0; tdevice < 0xFF; tdevice++) {
-                //Scan for access first
+                /* Scan for access first */
                 if (PCI_RefinedAccessConfig(ctrl->pci_bus, tdevice, 0x08, &work) == -1)
                         continue;
                 dbg("Looking for nonbridge bus_num %d dev_num %d\n", bus_num, tdevice);
-                //Yep we got one. Not a bridge ?
+                /* Yep we got one. Not a bridge ? */
                 if ((work >> 8) != PCI_TO_PCI_BRIDGE_CLASS) {
                         *dev_num = tdevice;
                         dbg("found it !\n");
@@ -225,16 +218,16 @@ static int PCI_ScanBusForNonBridge(struct controller *ctrl, u8 bus_num, u8 * dev
                 }
         }
         for (tdevice = 0; tdevice < 0xFF; tdevice++) {
-                //Scan for access first
+                /* Scan for access first */
                 if (PCI_RefinedAccessConfig(ctrl->pci_bus, tdevice, 0x08, &work) == -1)
                         continue;
                 dbg("Looking for bridge bus_num %d dev_num %d\n", bus_num, tdevice);
-                //Yep we got one. bridge ?
+                /* Yep we got one. bridge ? */
                 if ((work >> 8) == PCI_TO_PCI_BRIDGE_CLASS) {
                         pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(tdevice, 0), PCI_SECONDARY_BUS, &tbus);
+                        /* XXX: no recursion, wtf? */
                         dbg("Recurse on bus_num %d tdevice %d\n", tbus, tdevice);
-                        if (PCI_ScanBusNonBridge(tbus, tdevice) == 0)
-                                return 0;
+                        return 0;
                 }
         }
 
@@ -244,39 +237,23 @@ static int PCI_ScanBusForNonBridge(struct controller *ctrl, u8 bus_num, u8 * dev
 
 static int PCI_GetBusDevHelper(struct controller *ctrl, u8 *bus_num, u8 *dev_num, u8 slot, u8 nobridge)
 {
-        struct irq_routing_table *PCIIRQRoutingInfoLength;
-        long len;
-        long loop;
+        int loop, len;
         u32 work;
-
         u8 tbus, tdevice, tslot;
 
-        PCIIRQRoutingInfoLength = pcibios_get_irq_routing_table();
-        if (!PCIIRQRoutingInfoLength)
-                return -1;
-
-        len = (PCIIRQRoutingInfoLength->size -
-               sizeof(struct irq_routing_table)) / sizeof(struct irq_info);
-        // Make sure I got at least one entry
-        if (len == 0) {
-                kfree(PCIIRQRoutingInfoLength );
-                return -1;
-        }
-
+        len = cpqhp_routing_table_length();
         for (loop = 0; loop < len; ++loop) {
-                tbus = PCIIRQRoutingInfoLength->slots[loop].bus;
-                tdevice = PCIIRQRoutingInfoLength->slots[loop].devfn;
-                tslot = PCIIRQRoutingInfoLength->slots[loop].slot;
+                tbus = cpqhp_routing_table->slots[loop].bus;
+                tdevice = cpqhp_routing_table->slots[loop].devfn;
+                tslot = cpqhp_routing_table->slots[loop].slot;
 
                 if (tslot == slot) {
                         *bus_num = tbus;
                         *dev_num = tdevice;
                         ctrl->pci_bus->number = tbus;
                         pci_bus_read_config_dword (ctrl->pci_bus, *dev_num, PCI_VENDOR_ID, &work);
-                        if (!nobridge || (work == 0xffffffff)) {
-                                kfree(PCIIRQRoutingInfoLength );
+                        if (!nobridge || (work == 0xffffffff))
                                 return 0;
-                        }
 
                         dbg("bus_num %d devfn %d\n", *bus_num, *dev_num);
                         pci_bus_read_config_dword (ctrl->pci_bus, *dev_num, PCI_CLASS_REVISION, &work);
@@ -287,28 +264,26 @@ static int PCI_GetBusDevHelper(struct controller *ctrl, u8 *bus_num, u8 *dev_num
                                 dbg("Scan bus for Non Bridge: bus %d\n", tbus);
                                 if (PCI_ScanBusForNonBridge(ctrl, tbus, dev_num) == 0) {
                                         *bus_num = tbus;
-                                        kfree(PCIIRQRoutingInfoLength );
                                         return 0;
                                 }
-                        } else {
-                                kfree(PCIIRQRoutingInfoLength );
+                        } else
                                 return 0;
-                        }
-
                 }
         }
-        kfree(PCIIRQRoutingInfoLength );
         return -1;
 }
 
 
 int cpqhp_get_bus_dev (struct controller *ctrl, u8 * bus_num, u8 * dev_num, u8 slot)
 {
-        return PCI_GetBusDevHelper(ctrl, bus_num, dev_num, slot, 0);    //plain (bridges allowed)
+        /* plain (bridges allowed) */
+        return PCI_GetBusDevHelper(ctrl, bus_num, dev_num, slot, 0);
 }
 
 
-/* More PCI configuration routines; this time centered around hotplug controller */
+/* More PCI configuration routines; this time centered around hotplug
+ * controller
+ */
 
 
 /*
@@ -339,12 +314,12 @@ int cpqhp_save_config(struct controller *ctrl, int busnumber, int is_hot_plug)
         int stop_it;
         int index;
 
-        //              Decide which slots are supported
+        /* Decide which slots are supported */
 
         if (is_hot_plug) {
-                //*********************************
-                // is_hot_plug is the slot mask
-                //*********************************
+                /*
+                 * is_hot_plug is the slot mask
+                 */
                 FirstSupported = is_hot_plug >> 4;
                 LastSupported = FirstSupported + (is_hot_plug & 0x0F) - 1;
         } else {
@@ -352,123 +327,127 @@ int cpqhp_save_config(struct controller *ctrl, int busnumber, int is_hot_plug)
                 LastSupported = 0x1F;
         }
 
-        //     Save PCI configuration space for all devices in supported slots
+        /* Save PCI configuration space for all devices in supported slots */
         ctrl->pci_bus->number = busnumber;
         for (device = FirstSupported; device <= LastSupported; device++) {
                 ID = 0xFFFFFFFF;
-                rc = pci_bus_read_config_dword (ctrl->pci_bus, PCI_DEVFN(device, 0), PCI_VENDOR_ID, &ID);
+                rc = pci_bus_read_config_dword(ctrl->pci_bus, PCI_DEVFN(device, 0), PCI_VENDOR_ID, &ID);
+
+                if (ID == 0xFFFFFFFF) {
+                        if (is_hot_plug) {
+                                /* Setup slot structure with entry for empty
+                                 * slot
+                                 */
+                                new_slot = cpqhp_slot_create(busnumber);
+                                if (new_slot == NULL)
+                                        return 1;
 
-                if (ID != 0xFFFFFFFF) {   //  device in slot
-                        rc = pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(device, 0), 0x0B, &class_code);
-                        if (rc)
-                                return rc;
+                                new_slot->bus = (u8) busnumber;
+                                new_slot->device = (u8) device;
+                                new_slot->function = 0;
+                                new_slot->is_a_board = 0;
+                                new_slot->presence_save = 0;
+                                new_slot->switch_save = 0;
+                        }
+                        continue;
+                }
 
-                        rc = pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(device, 0), PCI_HEADER_TYPE, &header_type);
-                        if (rc)
-                                return rc;
+                rc = pci_bus_read_config_byte(ctrl->pci_bus, PCI_DEVFN(device, 0), 0x0B, &class_code);
+                if (rc)
+                        return rc;
 
-                        // If multi-function device, set max_functions to 8
-                        if (header_type & 0x80)
-                                max_functions = 8;
-                        else
-                                max_functions = 1;
+                rc = pci_bus_read_config_byte(ctrl->pci_bus, PCI_DEVFN(device, 0), PCI_HEADER_TYPE, &header_type);
+                if (rc)
+                        return rc;
 
-                        function = 0;
+                /* If multi-function device, set max_functions to 8 */
+                if (header_type & 0x80)
+                        max_functions = 8;
+                else
+                        max_functions = 1;
 
-                        do {
-                                DevError = 0;
+                function = 0;
 
-                                if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {   // P-P Bridge
-                                        //  Recurse the subordinate bus
-                                        //  get the subordinate bus number
-                                        rc = pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(device, function), PCI_SECONDARY_BUS, &secondary_bus);
-                                        if (rc) {
+                do {
+                        DevError = 0;
+                        if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
+                                /* Recurse the subordinate bus
+                                 * get the subordinate bus number
+                                 */
+                                rc = pci_bus_read_config_byte(ctrl->pci_bus, PCI_DEVFN(device, function), PCI_SECONDARY_BUS, &secondary_bus);
+                                if (rc) {
+                                        return rc;
+                                } else {
+                                        sub_bus = (int) secondary_bus;
+
+                                        /* Save secondary bus cfg spc
+                                         * with this recursive call.
+                                         */
+                                        rc = cpqhp_save_config(ctrl, sub_bus, 0);
+                                        if (rc)
                                                 return rc;
-                                        } else {
-                                                sub_bus = (int) secondary_bus;
-
-                                                // Save secondary bus cfg spc
-                                                // with this recursive call.
-                                                rc = cpqhp_save_config(ctrl, sub_bus, 0);
-                                                if (rc)
-                                                        return rc;
-                                                ctrl->pci_bus->number = busnumber;
-                                        }
+                                        ctrl->pci_bus->number = busnumber;
                                 }
+                        }
 
-                                index = 0;
+                        index = 0;
+                        new_slot = cpqhp_slot_find(busnumber, device, index++);
+                        while (new_slot &&
+                               (new_slot->function != (u8) function))
                                 new_slot = cpqhp_slot_find(busnumber, device, index++);
-                                while (new_slot && 
-                                       (new_slot->function != (u8) function))
-                                        new_slot = cpqhp_slot_find(busnumber, device, index++);
 
-                                if (!new_slot) {
-                                        // Setup slot structure.
-                                        new_slot = cpqhp_slot_create(busnumber);
-
-                                        if (new_slot == NULL)
-                                                return(1);
-                                }
-
-                                new_slot->bus = (u8) busnumber;
-                                new_slot->device = (u8) device;
-                                new_slot->function = (u8) function;
-                                new_slot->is_a_board = 1;
-                                new_slot->switch_save = 0x10;
-                                // In case of unsupported board
-                                new_slot->status = DevError;
-                                new_slot->pci_dev = pci_find_slot(new_slot->bus, (new_slot->device << 3) | new_slot->function);
-
-                                for (cloop = 0; cloop < 0x20; cloop++) {
-                                        rc = pci_bus_read_config_dword (ctrl->pci_bus, PCI_DEVFN(device, function), cloop << 2, (u32 *) & (new_slot-> config_space [cloop]));
-                                        if (rc)
-                                                return rc;
-                                }
+                        if (!new_slot) {
+                                /* Setup slot structure. */
+                                new_slot = cpqhp_slot_create(busnumber);
+                                if (new_slot == NULL)
+                                        return 1;
+                        }
 
-                                function++;
+                        new_slot->bus = (u8) busnumber;
+                        new_slot->device = (u8) device;
+                        new_slot->function = (u8) function;
+                        new_slot->is_a_board = 1;
+                        new_slot->switch_save = 0x10;
+                        /* In case of unsupported board */
+                        new_slot->status = DevError;
+                        new_slot->pci_dev = pci_get_bus_and_slot(new_slot->bus, (new_slot->device << 3) | new_slot->function);
 
-                                stop_it = 0;
+                        for (cloop = 0; cloop < 0x20; cloop++) {
+                                rc = pci_bus_read_config_dword(ctrl->pci_bus, PCI_DEVFN(device, function), cloop << 2, (u32 *) & (new_slot-> config_space [cloop]));
+                                if (rc)
+                                        return rc;
+                        }
 
-                                //  this loop skips to the next present function
-                                //  reading in Class Code and Header type.
+                        pci_dev_put(new_slot->pci_dev);
 
-                                while ((function < max_functions)&&(!stop_it)) {
-                                        rc = pci_bus_read_config_dword (ctrl->pci_bus, PCI_DEVFN(device, function), PCI_VENDOR_ID, &ID);
-                                        if (ID == 0xFFFFFFFF) {  // nothing there.
-                                                function++;
-                                        } else {  // Something there
-                                                rc = pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(device, function), 0x0B, &class_code);
-                                                if (rc)
-                                                        return rc;
+                        function++;
 
-                                                rc = pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(device, function), PCI_HEADER_TYPE, &header_type);
-                                                if (rc)
-                                                        return rc;
+                        stop_it = 0;
 
-                                                stop_it++;
-                                        }
+                        /* this loop skips to the next present function
+                         * reading in Class Code and Header type.
+                         */
+                        while ((function < max_functions) && (!stop_it)) {
+                                rc = pci_bus_read_config_dword(ctrl->pci_bus, PCI_DEVFN(device, function), PCI_VENDOR_ID, &ID);
+                                if (ID == 0xFFFFFFFF) {
+                                        function++;
+                                        continue;
                                 }
+                                rc = pci_bus_read_config_byte(ctrl->pci_bus, PCI_DEVFN(device, function), 0x0B, &class_code);
+                                if (rc)
+                                        return rc;
 
-                        } while (function < max_functions);
-                }               // End of IF (device in slot?)
-                else if (is_hot_plug) {
-                        // Setup slot structure with entry for empty slot
-                        new_slot = cpqhp_slot_create(busnumber);
+                                rc = pci_bus_read_config_byte(ctrl->pci_bus, PCI_DEVFN(device, function), PCI_HEADER_TYPE, &header_type);
+                                if (rc)
+                                        return rc;
 
-                        if (new_slot == NULL) {
-                                return(1);
+                                stop_it++;
                         }
 
-                        new_slot->bus = (u8) busnumber;
-                        new_slot->device = (u8) device;
-                        new_slot->function = 0;
-                        new_slot->is_a_board = 0;
-                        new_slot->presence_save = 0;
-                        new_slot->switch_save = 0;
-                }
-        }                       // End of FOR loop
+                } while (function < max_functions);
+        }                       /* End of FOR loop */
 
-        return(0);
+        return 0;
 }
 
 
@@ -489,7 +468,7 @@ int cpqhp_save_slot_config (struct controller *ctrl, struct pci_func * new_slot)
         u8 secondary_bus;
         int sub_bus;
         int max_functions;
-        int function;
+        int function = 0;
         int cloop = 0;
         int stop_it;
 
@@ -498,63 +477,58 @@ int cpqhp_save_slot_config (struct controller *ctrl, struct pci_func * new_slot)
         ctrl->pci_bus->number = new_slot->bus;
         pci_bus_read_config_dword (ctrl->pci_bus, PCI_DEVFN(new_slot->device, 0), PCI_VENDOR_ID, &ID);
 
-        if (ID != 0xFFFFFFFF) {   //  device in slot
-                pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(new_slot->device, 0), 0x0B, &class_code);
-                pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(new_slot->device, 0), PCI_HEADER_TYPE, &header_type);
-
-                if (header_type & 0x80) // Multi-function device
-                        max_functions = 8;
-                else
-                        max_functions = 1;
-
-                function = 0;
-
-                do {
-                        if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {     // PCI-PCI Bridge
-                                //  Recurse the subordinate bus
-                                pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), PCI_SECONDARY_BUS, &secondary_bus);
+        if (ID == 0xFFFFFFFF)
+                return 2;
 
-                                sub_bus = (int) secondary_bus;
+        pci_bus_read_config_byte(ctrl->pci_bus, PCI_DEVFN(new_slot->device, 0), 0x0B, &class_code);
+        pci_bus_read_config_byte(ctrl->pci_bus, PCI_DEVFN(new_slot->device, 0), PCI_HEADER_TYPE, &header_type);
 
-                                // Save the config headers for the secondary bus.
-                                rc = cpqhp_save_config(ctrl, sub_bus, 0);
-                                if (rc)
-                                        return(rc);
-                                ctrl->pci_bus->number = new_slot->bus;
+        if (header_type & 0x80) /* Multi-function device */
+                max_functions = 8;
+        else
+                max_functions = 1;
 
-                        }       // End of IF
+        while (function < max_functions) {
+                if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
+                        /*  Recurse the subordinate bus */
+                        pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), PCI_SECONDARY_BUS, &secondary_bus);
 
-                        new_slot->status = 0;
+                        sub_bus = (int) secondary_bus;
 
-                        for (cloop = 0; cloop < 0x20; cloop++) {
-                                pci_bus_read_config_dword (ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), cloop << 2, (u32 *) & (new_slot-> config_space [cloop]));
-                        }
+                        /* Save the config headers for the secondary
+                         * bus.
+                         */
+                        rc = cpqhp_save_config(ctrl, sub_bus, 0);
+                        if (rc)
+                                return(rc);
+                        ctrl->pci_bus->number = new_slot->bus;
 
-                        function++;
+                }
 
-                        stop_it = 0;
+                new_slot->status = 0;
 
-                        //  this loop skips to the next present function
-                        //  reading in the Class Code and the Header type.
+                for (cloop = 0; cloop < 0x20; cloop++)
+                        pci_bus_read_config_dword(ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), cloop << 2, (u32 *) & (new_slot-> config_space [cloop]));
 
-                        while ((function < max_functions) && (!stop_it)) {
-                                pci_bus_read_config_dword (ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), PCI_VENDOR_ID, &ID);
+                function++;
 
-                                if (ID == 0xFFFFFFFF) {  // nothing there.
-                                        function++;
-                                } else {  // Something there
-                                        pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), 0x0B, &class_code);
+                stop_it = 0;
 
-                                        pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), PCI_HEADER_TYPE, &header_type);
+                /* this loop skips to the next present function
+                 * reading in the Class Code and the Header type.
+                 */
+                while ((function < max_functions) && (!stop_it)) {
+                        pci_bus_read_config_dword(ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), PCI_VENDOR_ID, &ID);
 
-                                        stop_it++;
-                                }
+                        if (ID == 0xFFFFFFFF)
+                                function++;
+                        else {
+                                pci_bus_read_config_byte(ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), 0x0B, &class_code);
+                                pci_bus_read_config_byte(ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), PCI_HEADER_TYPE, &header_type);
+                                stop_it++;
                         }
+                }
 
-                } while (function < max_functions);
-        }                       // End of IF (device in slot?)
-        else {
-                return 2;
         }
 
         return 0;
@@ -590,11 +564,10 @@ int cpqhp_save_base_addr_length(struct controller *ctrl, struct pci_func * func)
                 pci_bus->number = func->bus;
                 devfn = PCI_DEVFN(func->device, func->function);
 
-                // Check for Bridge
+                /* Check for Bridge */
                 pci_bus_read_config_byte (pci_bus, devfn, PCI_HEADER_TYPE, &header_type);
 
                 if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
-                        // PCI-PCI Bridge
                         pci_bus_read_config_byte (pci_bus, devfn, PCI_SECONDARY_BUS, &secondary_bus);
 
                         sub_bus = (int) secondary_bus;
@@ -610,23 +583,27 @@ int cpqhp_save_base_addr_length(struct controller *ctrl, struct pci_func * func)
                         }
                         pci_bus->number = func->bus;
 
-                        //FIXME: this loop is duplicated in the non-bridge case.  The two could be rolled together
-                        // Figure out IO and memory base lengths
+                        /* FIXME: this loop is duplicated in the non-bridge
+                         * case.  The two could be rolled together Figure out
+                         * IO and memory base lengths
+                         */
                         for (cloop = 0x10; cloop <= 0x14; cloop += 4) {
                                 temp_register = 0xFFFFFFFF;
                                 pci_bus_write_config_dword (pci_bus, devfn, cloop, temp_register);
                                 pci_bus_read_config_dword (pci_bus, devfn, cloop, &base);
-
-                                if (base) {  // If this register is implemented
+                                /* If this register is implemented */
+                                if (base) {
                                         if (base & 0x01L) {
-                                                // IO base
-                                                // set base = amount of IO space requested
+                                                /* IO base
+                                                 * set base = amount of IO space
+                                                 * requested
+                                                 */
                                                 base = base & 0xFFFFFFFE;
                                                 base = (~base) + 1;
 
                                                 type = 1;
                                         } else {
-                                                // memory base
+                                                /* memory base */
                                                 base = base & 0xFFFFFFF0;
                                                 base = (~base) + 1;
 
@@ -637,32 +614,36 @@ int cpqhp_save_base_addr_length(struct controller *ctrl, struct pci_func * func)
                                         type = 0;
                                 }
 
-                                // Save information in slot structure
+                                /* Save information in slot structure */
                                 func->base_length[(cloop - 0x10) >> 2] =
                                 base;
                                 func->base_type[(cloop - 0x10) >> 2] = type;
 
-                        }       // End of base register loop
+                        }       /* End of base register loop */
 
-
-                } else if ((header_type & 0x7F) == 0x00) {        // PCI-PCI Bridge
-                        // Figure out IO and memory base lengths
+                } else if ((header_type & 0x7F) == 0x00) {
+                        /* Figure out IO and memory base lengths */
                         for (cloop = 0x10; cloop <= 0x24; cloop += 4) {
                                 temp_register = 0xFFFFFFFF;
                                 pci_bus_write_config_dword (pci_bus, devfn, cloop, temp_register);
                                 pci_bus_read_config_dword (pci_bus, devfn, cloop, &base);
 
-                                if (base) {  // If this register is implemented
+                                /* If this register is implemented */
+                                if (base) {
                                         if (base & 0x01L) {
-                                                // IO base
-                                                // base = amount of IO space requested
+                                                /* IO base
+                                                 * base = amount of IO space
+                                                 * requested
+                                                 */
                                                 base = base & 0xFFFFFFFE;
                                                 base = (~base) + 1;
 
                                                 type = 1;
                                         } else {
-                                                // memory base
-                                                // base = amount of memory space requested
+                                                /* memory base
+                                                 * base = amount of memory
+                                                 * space requested
+                                                 */
                                                 base = base & 0xFFFFFFF0;
                                                 base = (~base) + 1;
 
@@ -673,16 +654,16 @@ int cpqhp_save_base_addr_length(struct controller *ctrl, struct pci_func * func)
                                         type = 0;
                                 }
 
-                                // Save information in slot structure
+                                /* Save information in slot structure */
                                 func->base_length[(cloop - 0x10) >> 2] = base;
                                 func->base_type[(cloop - 0x10) >> 2] = type;
 
-                        }       // End of base register loop
+                        }       /* End of base register loop */
 
-                } else {          // Some other unknown header type
+                } else {          /* Some other unknown header type */
                 }
 
-                // find the next device in this slot
+                /* find the next device in this slot */
                 func = cpqhp_slot_find(func->bus, func->device, index++);
         }
 
@@ -728,18 +709,18 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
                 pci_bus->number = func->bus;
                 devfn = PCI_DEVFN(func->device, func->function);
 
-                // Save the command register
+                /* Save the command register */
                 pci_bus_read_config_word(pci_bus, devfn, PCI_COMMAND, &save_command);
 
-                // disable card
+                /* disable card */
                 command = 0x00;
                 pci_bus_write_config_word(pci_bus, devfn, PCI_COMMAND, command);
 
-                // Check for Bridge
+                /* Check for Bridge */
                 pci_bus_read_config_byte(pci_bus, devfn, PCI_HEADER_TYPE, &header_type);
 
-                if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {     // PCI-PCI Bridge
-                        // Clear Bridge Control Register
+                if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
+                        /* Clear Bridge Control Register */
                         command = 0x00;
                         pci_bus_write_config_word(pci_bus, devfn, PCI_BRIDGE_CONTROL, command);
                         pci_bus_read_config_byte(pci_bus, devfn, PCI_SECONDARY_BUS, &secondary_bus);
@@ -755,7 +736,7 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
                         bus_node->next = func->bus_head;
                         func->bus_head = bus_node;
 
-                        // Save IO base and Limit registers
+                        /* Save IO base and Limit registers */
                         pci_bus_read_config_byte(pci_bus, devfn, PCI_IO_BASE, &b_base);
                         pci_bus_read_config_byte(pci_bus, devfn, PCI_IO_LIMIT, &b_length);
 
@@ -771,7 +752,7 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
                                 func->io_head = io_node;
                         }
 
-                        // Save memory base and Limit registers
+                        /* Save memory base and Limit registers */
                         pci_bus_read_config_word(pci_bus, devfn, PCI_MEMORY_BASE, &w_base);
                         pci_bus_read_config_word(pci_bus, devfn, PCI_MEMORY_LIMIT, &w_length);
 
@@ -787,7 +768,7 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
                                 func->mem_head = mem_node;
                         }
 
-                        // Save prefetchable memory base and Limit registers
+                        /* Save prefetchable memory base and Limit registers */
                         pci_bus_read_config_word(pci_bus, devfn, PCI_PREF_MEMORY_BASE, &w_base);
                         pci_bus_read_config_word(pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, &w_length);
 
@@ -802,7 +783,7 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
                                 p_mem_node->next = func->p_mem_head;
                                 func->p_mem_head = p_mem_node;
                         }
-                        // Figure out IO and memory base lengths
+                        /* Figure out IO and memory base lengths */
                         for (cloop = 0x10; cloop <= 0x14; cloop += 4) {
                                 pci_bus_read_config_dword (pci_bus, devfn, cloop, &save_base);
 
@@ -812,11 +793,14 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
 
                                 temp_register = base;
 
-                                if (base) {  // If this register is implemented
+                                /* If this register is implemented */
+                                if (base) {
                                         if (((base & 0x03L) == 0x01)
                                             && (save_command & 0x01)) {
-                                                // IO base
-                                                // set temp_register = amount of IO space requested
+                                                /* IO base
+                                                 * set temp_register = amount
+                                                 * of IO space requested
+                                                 */
                                                 temp_register = base & 0xFFFFFFFE;
                                                 temp_register = (~temp_register) + 1;
 
@@ -834,7 +818,7 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
                                         } else
                                                 if (((base & 0x0BL) == 0x08)
                                                     && (save_command & 0x02)) {
-                                                // prefetchable memory base
+                                                /* prefetchable memory base */
                                                 temp_register = base & 0xFFFFFFF0;
                                                 temp_register = (~temp_register) + 1;
 
@@ -851,7 +835,7 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
                                         } else
                                                 if (((base & 0x0BL) == 0x00)
                                                     && (save_command & 0x02)) {
-                                                // prefetchable memory base
+                                                /* prefetchable memory base */
                                                 temp_register = base & 0xFFFFFFF0;
                                                 temp_register = (~temp_register) + 1;
 
@@ -868,9 +852,10 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
                                         } else
                                                 return(1);
                                 }
-                        }       // End of base register loop
-                } else if ((header_type & 0x7F) == 0x00) {        // Standard header
-                        // Figure out IO and memory base lengths
+                        }       /* End of base register loop */
+                /* Standard header */
+                } else if ((header_type & 0x7F) == 0x00) {
+                        /* Figure out IO and memory base lengths */
                         for (cloop = 0x10; cloop <= 0x24; cloop += 4) {
                                 pci_bus_read_config_dword(pci_bus, devfn, cloop, &save_base);
 
@@ -880,11 +865,14 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
 
                                 temp_register = base;
 
-                                if (base) {       // If this register is implemented
+                                /* If this register is implemented */
+                                if (base) {
                                         if (((base & 0x03L) == 0x01)
                                             && (save_command & 0x01)) {
-                                                // IO base
-                                                // set temp_register = amount of IO space requested
+                                                /* IO base
+                                                 * set temp_register = amount
+                                                 * of IO space requested
+                                                 */
                                                 temp_register = base & 0xFFFFFFFE;
                                                 temp_register = (~temp_register) + 1;
 
@@ -901,7 +889,7 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
                                         } else
                                                 if (((base & 0x0BL) == 0x08)
                                                     && (save_command & 0x02)) {
-                                                // prefetchable memory base
+                                                /* prefetchable memory base */
                                                 temp_register = base & 0xFFFFFFF0;
                                                 temp_register = (~temp_register) + 1;
 
@@ -918,7 +906,7 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
                                         } else
                                                 if (((base & 0x0BL) == 0x00)
                                                     && (save_command & 0x02)) {
-                                                // prefetchable memory base
+                                                /* prefetchable memory base */
                                                 temp_register = base & 0xFFFFFFF0;
                                                 temp_register = (~temp_register) + 1;
 
@@ -935,15 +923,14 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func * func)
                                         } else
                                                 return(1);
                                 }
-                        }       // End of base register loop
-                } else {          // Some other unknown header type
+                        }       /* End of base register loop */
                 }
 
-                // find the next device in this slot
+                /* find the next device in this slot */
                 func = cpqhp_slot_find(func->bus, func->device, index++);
         }
 
-        return(0);
+        return 0;
 }
 
 
@@ -975,16 +962,16 @@ int cpqhp_configure_board(struct controller *ctrl, struct pci_func * func)
                 pci_bus->number = func->bus;
                 devfn = PCI_DEVFN(func->device, func->function);
 
-                // Start at the top of config space so that the control
-                // registers are programmed last
-                for (cloop = 0x3C; cloop > 0; cloop -= 4) {
+                /* Start at the top of config space so that the control
+                 * registers are programmed last
+                 */
+                for (cloop = 0x3C; cloop > 0; cloop -= 4)
                         pci_bus_write_config_dword (pci_bus, devfn, cloop, func->config_space[cloop >> 2]);
-                }
 
                 pci_bus_read_config_byte (pci_bus, devfn, PCI_HEADER_TYPE, &header_type);
 
-                // If this is a bridge device, restore subordinate devices
-                if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {     // PCI-PCI Bridge
+                /* If this is a bridge device, restore subordinate devices */
+                if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
                         pci_bus_read_config_byte (pci_bus, devfn, PCI_SECONDARY_BUS, &secondary_bus);
 
                         sub_bus = (int) secondary_bus;
@@ -1000,8 +987,9 @@ int cpqhp_configure_board(struct controller *ctrl, struct pci_func * func)
                         }
                 } else {
 
-                        // Check all the base Address Registers to make sure
-                        // they are the same.  If not, the board is different.
+                        /* Check all the base Address Registers to make sure
+                         * they are the same.  If not, the board is different.
+                         */
 
                         for (cloop = 16; cloop < 40; cloop += 4) {
                                 pci_bus_read_config_dword (pci_bus, devfn, cloop, &temp);
@@ -1058,27 +1046,28 @@ int cpqhp_valid_replace(struct controller *ctrl, struct pci_func * func)
 
                 pci_bus_read_config_dword (pci_bus, devfn, PCI_VENDOR_ID, &temp_register);
 
-                // No adapter present
+                /* No adapter present */
                 if (temp_register == 0xFFFFFFFF)
                         return(NO_ADAPTER_PRESENT);
 
                 if (temp_register != func->config_space[0])
                         return(ADAPTER_NOT_SAME);
 
-                // Check for same revision number and class code
+                /* Check for same revision number and class code */
                 pci_bus_read_config_dword (pci_bus, devfn, PCI_CLASS_REVISION, &temp_register);
 
-                // Adapter not the same
+                /* Adapter not the same */
                 if (temp_register != func->config_space[0x08 >> 2])
                         return(ADAPTER_NOT_SAME);
 
-                // Check for Bridge
+                /* Check for Bridge */
                 pci_bus_read_config_byte (pci_bus, devfn, PCI_HEADER_TYPE, &header_type);
 
-                if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {     // PCI-PCI Bridge
-                        // In order to continue checking, we must program the
-                        // bus registers in the bridge to respond to accesses
-                        // for it's subordinate bus(es)
+                if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
+                        /* In order to continue checking, we must program the
+                         * bus registers in the bridge to respond to accesses
+                         * for its subordinate bus(es)
+                         */
 
                         temp_register = func->config_space[0x18 >> 2];
                         pci_bus_write_config_dword (pci_bus, devfn, PCI_PRIMARY_BUS, temp_register);
@@ -1096,35 +1085,39 @@ int cpqhp_valid_replace(struct controller *ctrl, struct pci_func * func)
                         }
 
                 }
-                // Check to see if it is a standard config header
+                /* Check to see if it is a standard config header */
                 else if ((header_type & 0x7F) == PCI_HEADER_TYPE_NORMAL) {
-                        // Check subsystem vendor and ID
+                        /* Check subsystem vendor and ID */
                         pci_bus_read_config_dword (pci_bus, devfn, PCI_SUBSYSTEM_VENDOR_ID, &temp_register);
 
                         if (temp_register != func->config_space[0x2C >> 2]) {
-                                // If it's a SMART-2 and the register isn't filled
-                                // in, ignore the difference because
-                                // they just have an old rev of the firmware
-
+                                /* If it's a SMART-2 and the register isn't
+                                 * filled in, ignore the difference because
+                                 * they just have an old rev of the firmware
+                                 */
                                 if (!((func->config_space[0] == 0xAE100E11)
                                       && (temp_register == 0x00L)))
                                         return(ADAPTER_NOT_SAME);
                         }
-                        // Figure out IO and memory base lengths
+                        /* Figure out IO and memory base lengths */
                         for (cloop = 0x10; cloop <= 0x24; cloop += 4) {
                                 temp_register = 0xFFFFFFFF;
                                 pci_bus_write_config_dword (pci_bus, devfn, cloop, temp_register);
                                 pci_bus_read_config_dword (pci_bus, devfn, cloop, &base);
-                                if (base) {       // If this register is implemented
+
+                                /* If this register is implemented */
+                                if (base) {
                                         if (base & 0x01L) {
-                                                // IO base
-                                                // set base = amount of IO space requested
+                                                /* IO base
+                                                 * set base = amount of IO
+                                                 * space requested
+                                                 */
                                                 base = base & 0xFFFFFFFE;
                                                 base = (~base) + 1;
 
                                                 type = 1;
                                         } else {
-                                                // memory base
+                                                /* memory base */
                                                 base = base & 0xFFFFFFF0;
                                                 base = (~base) + 1;
 
@@ -1135,23 +1128,24 @@ int cpqhp_valid_replace(struct controller *ctrl, struct pci_func * func)
                                         type = 0;
                                 }
 
-                                // Check information in slot structure
+                                /* Check information in slot structure */
                                 if (func->base_length[(cloop - 0x10) >> 2] != base)
                                         return(ADAPTER_NOT_SAME);
 
                                 if (func->base_type[(cloop - 0x10) >> 2] != type)
                                         return(ADAPTER_NOT_SAME);
 
-                        }       // End of base register loop
+                        }       /* End of base register loop */
 
-                }               // End of (type 0 config space) else
+                }               /* End of (type 0 config space) else */
                 else {
-                        // this is not a type 0 or 1 config space header so
-                        // we don't know how to do it
+                        /* this is not a type 0 or 1 config space header so
+                         * we don't know how to do it
+                         */
                         return(DEVICE_TYPE_NOT_SUPPORTED);
                 }
 
-                // Get the next function
+                /* Get the next function */
                 func = cpqhp_slot_find(func->bus, func->device, index++);
         }
 
@@ -1168,7 +1162,7 @@ int cpqhp_valid_replace(struct controller *ctrl, struct pci_func * func)
  * this function is for hot plug ADD!
  *
  * returns 0 if success
- */  
+ */
 int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_start)
 {
         u8 temp;
@@ -1187,10 +1181,10 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
         rom_resource_table = detect_HRT_floating_pointer(rom_start, rom_start+0xffff);
         dbg("rom_resource_table = %p\n", rom_resource_table);
 
-        if (rom_resource_table == NULL) {
+        if (rom_resource_table == NULL)
                 return -ENODEV;
-        }
-        // Sum all resources and setup resource maps
+
+        /* Sum all resources and setup resource maps */
         unused_IRQ = readl(rom_resource_table + UNUSED_IRQ);
         dbg("unused_IRQ = %x\n", unused_IRQ);
 
@@ -1222,13 +1216,11 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
 
         temp = 0;
 
-        if (!cpqhp_nic_irq) {
+        if (!cpqhp_nic_irq)
                 cpqhp_nic_irq = ctrl->cfgspc_irq;
-        }
 
-        if (!cpqhp_disk_irq) {
+        if (!cpqhp_disk_irq)
                 cpqhp_disk_irq = ctrl->cfgspc_irq;
-        }
 
         dbg("cpqhp_disk_irq, cpqhp_nic_irq= %d, %d\n", cpqhp_disk_irq, cpqhp_nic_irq);
 
@@ -1262,13 +1254,13 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
                     dev_func, io_base, io_length, mem_base, mem_length, pre_mem_base, pre_mem_length,
                     primary_bus, secondary_bus, max_bus);
 
-                // If this entry isn't for our controller's bus, ignore it
+                /* If this entry isn't for our controller's bus, ignore it */
                 if (primary_bus != ctrl->bus) {
                         i--;
                         one_slot += sizeof (struct slot_rt);
                         continue;
                 }
-                // find out if this entry is for an occupied slot
+                /* find out if this entry is for an occupied slot */
                 ctrl->pci_bus->number = primary_bus;
                 pci_bus_read_config_dword (ctrl->pci_bus, dev_func, PCI_VENDOR_ID, &temp_dword);
                 dbg("temp_D_word = %x\n", temp_dword);
@@ -1282,13 +1274,13 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
                                 func = cpqhp_slot_find(primary_bus, dev_func >> 3, index++);
                         }
 
-                        // If we can't find a match, skip this table entry
+                        /* If we can't find a match, skip this table entry */
                         if (!func) {
                                 i--;
                                 one_slot += sizeof (struct slot_rt);
                                 continue;
                         }
-                        // this may not work and shouldn't be used
+                        /* this may not work and shouldn't be used */
                         if (secondary_bus != primary_bus)
                                 bridged_slot = 1;
                         else
@@ -1301,7 +1293,7 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
                 }
 
 
-                // If we've got a valid IO base, use it
+                /* If we've got a valid IO base, use it */
 
                 temp_dword = io_base + io_length;
 
@@ -1325,7 +1317,7 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
                         }
                 }
 
-                // If we've got a valid memory base, use it
+                /* If we've got a valid memory base, use it */
                 temp_dword = mem_base + mem_length;
                 if ((mem_base) && (temp_dword < 0x10000)) {
                         mem_node = kmalloc(sizeof(*mem_node), GFP_KERNEL);
@@ -1348,8 +1340,9 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
                         }
                 }
 
-                // If we've got a valid prefetchable memory base, and
-                // the base + length isn't greater than 0xFFFF
+                /* If we've got a valid prefetchable memory base, and
+                 * the base + length isn't greater than 0xFFFF
+                 */
                 temp_dword = pre_mem_base + pre_mem_length;
                 if ((pre_mem_base) && (temp_dword < 0x10000)) {
                         p_mem_node = kmalloc(sizeof(*p_mem_node), GFP_KERNEL);
@@ -1372,9 +1365,10 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
                         }
                 }
 
-                // If we've got a valid bus number, use it
-                // The second condition is to ignore bus numbers on
-                // populated slots that don't have PCI-PCI bridges
+                /* If we've got a valid bus number, use it
+                 * The second condition is to ignore bus numbers on
+                 * populated slots that don't have PCI-PCI bridges
+                 */
                 if (secondary_bus && (secondary_bus != primary_bus)) {
                         bus_node = kmalloc(sizeof(*bus_node), GFP_KERNEL);
                         if (!bus_node)
@@ -1398,8 +1392,9 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
                 one_slot += sizeof (struct slot_rt);
         }
 
-        // If all of the following fail, we don't have any resources for
-        // hot plug add
+        /* If all of the following fail, we don't have any resources for
+         * hot plug add
+         */
         rc = 1;
         rc &= cpqhp_resource_sort_and_combine(&(ctrl->mem_head));
         rc &= cpqhp_resource_sort_and_combine(&(ctrl->p_mem_head));
diff --git a/drivers/pci/hotplug/ibmphp_core.c b/drivers/pci/hotplug/ibmphp_core.c
index 42e4260c3b12..7485ffda950c 100644
--- a/drivers/pci/hotplug/ibmphp_core.c
+++ b/drivers/pci/hotplug/ibmphp_core.c
@@ -1318,7 +1318,6 @@ error:
 }
 
 struct hotplug_slot_ops ibmphp_hotplug_slot_ops = {
-        .owner =                        THIS_MODULE,
         .set_attention_status =         set_attention_status,
         .enable_slot =                  enable_slot,
         .disable_slot =                 ibmphp_disable_slot,
@@ -1421,3 +1420,4 @@ static void __exit ibmphp_exit(void)
 }
 
 module_init(ibmphp_init);
+module_exit(ibmphp_exit);
diff --git a/drivers/pci/hotplug/pci_hotplug_core.c b/drivers/pci/hotplug/pci_hotplug_core.c
index 535fce0f07f9..844580489d4d 100644
--- a/drivers/pci/hotplug/pci_hotplug_core.c
+++ b/drivers/pci/hotplug/pci_hotplug_core.c
@@ -347,125 +347,129 @@ static struct pci_slot_attribute hotplug_slot_attr_test = {
         .store = test_write_file
 };
 
-static int has_power_file(struct pci_slot *pci_slot)
+static bool has_power_file(struct pci_slot *pci_slot)
 {
         struct hotplug_slot *slot = pci_slot->hotplug;
         if ((!slot) || (!slot->ops))
-                return -ENODEV;
+                return false;
         if ((slot->ops->enable_slot) ||
             (slot->ops->disable_slot) ||
             (slot->ops->get_power_status))
-                return 0;
-        return -ENOENT;
+                return true;
+        return false;
 }
 
-static int has_attention_file(struct pci_slot *pci_slot)
+static bool has_attention_file(struct pci_slot *pci_slot)
 {
         struct hotplug_slot *slot = pci_slot->hotplug;
         if ((!slot) || (!slot->ops))
-                return -ENODEV;
+                return false;
         if ((slot->ops->set_attention_status) ||
             (slot->ops->get_attention_status))
-                return 0;
-        return -ENOENT;
+                return true;
+        return false;
 }
 
-static int has_latch_file(struct pci_slot *pci_slot)
+static bool has_latch_file(struct pci_slot *pci_slot)
 {
         struct hotplug_slot *slot = pci_slot->hotplug;
         if ((!slot) || (!slot->ops))
-                return -ENODEV;
+                return false;
         if (slot->ops->get_latch_status)
-                return 0;
-        return -ENOENT;
+                return true;
+        return false;
 }
 
-static int has_adapter_file(struct pci_slot *pci_slot)
+static bool has_adapter_file(struct pci_slot *pci_slot)
 {
         struct hotplug_slot *slot = pci_slot->hotplug;
         if ((!slot) || (!slot->ops))
-                return -ENODEV;
+                return false;
         if (slot->ops->get_adapter_status)
-                return 0;
-        return -ENOENT;
+                return true;
+        return false;
 }
 
-static int has_max_bus_speed_file(struct pci_slot *pci_slot)
+static bool has_max_bus_speed_file(struct pci_slot *pci_slot)
 {
         struct hotplug_slot *slot = pci_slot->hotplug;
         if ((!slot) || (!slot->ops))
-                return -ENODEV;
+                return false;
         if (slot->ops->get_max_bus_speed)
-                return 0;
-        return -ENOENT;
+                return true;
+        return false;
 }
 
-static int has_cur_bus_speed_file(struct pci_slot *pci_slot)
+static bool has_cur_bus_speed_file(struct pci_slot *pci_slot)
 {
         struct hotplug_slot *slot = pci_slot->hotplug;
         if ((!slot) || (!slot->ops))
-                return -ENODEV;
+                return false;
         if (slot->ops->get_cur_bus_speed)
-                return 0;
-        return -ENOENT;
+                return true;
+        return false;
 }
 
-static int has_test_file(struct pci_slot *pci_slot)
+static bool has_test_file(struct pci_slot *pci_slot)
 {
         struct hotplug_slot *slot = pci_slot->hotplug;
         if ((!slot) || (!slot->ops))
-                return -ENODEV;
+                return false;
         if (slot->ops->hardware_test)
-                return 0;
-        return -ENOENT;
+                return true;
+        return false;
 }
 
 static int fs_add_slot(struct pci_slot *slot)
 {
         int retval = 0;
 
-        if (has_power_file(slot) == 0) {
-                retval = sysfs_create_file(&slot->kobj, &hotplug_slot_attr_power.attr);
+        /* Create symbolic link to the hotplug driver module */
+        pci_hp_create_module_link(slot);
+
+        if (has_power_file(slot)) {
+                retval = sysfs_create_file(&slot->kobj,
+                                           &hotplug_slot_attr_power.attr);
                 if (retval)
                         goto exit_power;
         }
 
-        if (has_attention_file(slot) == 0) {
+        if (has_attention_file(slot)) {
                 retval = sysfs_create_file(&slot->kobj,
                                            &hotplug_slot_attr_attention.attr);
                 if (retval)
                         goto exit_attention;
         }
 
-        if (has_latch_file(slot) == 0) {
+        if (has_latch_file(slot)) {
                 retval = sysfs_create_file(&slot->kobj,
                                            &hotplug_slot_attr_latch.attr);
                 if (retval)
                         goto exit_latch;
         }
 
-        if (has_adapter_file(slot) == 0) {
+        if (has_adapter_file(slot)) {
                 retval = sysfs_create_file(&slot->kobj,
                                            &hotplug_slot_attr_presence.attr);
                 if (retval)
                         goto exit_adapter;
         }
 
-        if (has_max_bus_speed_file(slot) == 0) {
+        if (has_max_bus_speed_file(slot)) {
                 retval = sysfs_create_file(&slot->kobj,
-                                           &hotplug_slot_attr_max_bus_speed.attr);
+                                        &hotplug_slot_attr_max_bus_speed.attr);
                 if (retval)
                         goto exit_max_speed;
         }
 
-        if (has_cur_bus_speed_file(slot) == 0) {
+        if (has_cur_bus_speed_file(slot)) {
                 retval = sysfs_create_file(&slot->kobj,
-                                           &hotplug_slot_attr_cur_bus_speed.attr);
+                                        &hotplug_slot_attr_cur_bus_speed.attr);
                 if (retval)
                         goto exit_cur_speed;
         }
 
-        if (has_test_file(slot) == 0) {
+        if (has_test_file(slot)) {
                 retval = sysfs_create_file(&slot->kobj,
                                            &hotplug_slot_attr_test.attr);
                 if (retval)
@@ -475,55 +479,61 @@ static int fs_add_slot(struct pci_slot *slot)
         goto exit;
 
 exit_test:
-        if (has_cur_bus_speed_file(slot) == 0)
-                sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_cur_bus_speed.attr);
-
+        if (has_cur_bus_speed_file(slot))
+                sysfs_remove_file(&slot->kobj,
+                                  &hotplug_slot_attr_cur_bus_speed.attr);
 exit_cur_speed:
-        if (has_max_bus_speed_file(slot) == 0)
-                sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_max_bus_speed.attr);
-
+        if (has_max_bus_speed_file(slot))
+                sysfs_remove_file(&slot->kobj,
+                                  &hotplug_slot_attr_max_bus_speed.attr);
 exit_max_speed:
-        if (has_adapter_file(slot) == 0)
-                sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_presence.attr);
-
+        if (has_adapter_file(slot))
+                sysfs_remove_file(&slot->kobj,
+                                  &hotplug_slot_attr_presence.attr);
 exit_adapter:
-        if (has_latch_file(slot) == 0)
+        if (has_latch_file(slot))
                 sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_latch.attr);
-
 exit_latch:
-        if (has_attention_file(slot) == 0)
-                sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_attention.attr);
-
+        if (has_attention_file(slot))
+                sysfs_remove_file(&slot->kobj,
+                                  &hotplug_slot_attr_attention.attr);
 exit_attention:
-        if (has_power_file(slot) == 0)
+        if (has_power_file(slot))
                 sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_power.attr);
 exit_power:
+        pci_hp_remove_module_link(slot);
 exit:
         return retval;
 }
 
 static void fs_remove_slot(struct pci_slot *slot)
 {
-        if (has_power_file(slot) == 0)
+        if (has_power_file(slot))
                 sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_power.attr);
 
-        if (has_attention_file(slot) == 0)
-                sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_attention.attr);
+        if (has_attention_file(slot))
+                sysfs_remove_file(&slot->kobj,
+                                  &hotplug_slot_attr_attention.attr);
 
-        if (has_latch_file(slot) == 0)
+        if (has_latch_file(slot))
                 sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_latch.attr);
 
-        if (has_adapter_file(slot) == 0)
-                sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_presence.attr);
+        if (has_adapter_file(slot))
+                sysfs_remove_file(&slot->kobj,
+                                  &hotplug_slot_attr_presence.attr);
 
-        if (has_max_bus_speed_file(slot) == 0)
-                sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_max_bus_speed.attr);
+        if (has_max_bus_speed_file(slot))
+                sysfs_remove_file(&slot->kobj,
+                                  &hotplug_slot_attr_max_bus_speed.attr);
 
-        if (has_cur_bus_speed_file(slot) == 0)
-                sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_cur_bus_speed.attr);
+        if (has_cur_bus_speed_file(slot))
+                sysfs_remove_file(&slot->kobj,
+                                  &hotplug_slot_attr_cur_bus_speed.attr);
 
-        if (has_test_file(slot) == 0)
+        if (has_test_file(slot))
                 sysfs_remove_file(&slot->kobj, &hotplug_slot_attr_test.attr);
+
+        pci_hp_remove_module_link(slot);
 }
 
 static struct hotplug_slot *get_slot_from_name (const char *name)
@@ -540,10 +550,10 @@ static struct hotplug_slot *get_slot_from_name (const char *name)
 }
 
 /**
- * pci_hp_register - register a hotplug_slot with the PCI hotplug subsystem
+ * __pci_hp_register - register a hotplug_slot with the PCI hotplug subsystem
  * @bus: bus this slot is on
  * @slot: pointer to the &struct hotplug_slot to register
- * @slot_nr: slot number
+ * @devnr: device number
  * @name: name registered with kobject core
  *
  * Registers a hotplug slot with the pci hotplug subsystem, which will allow
@@ -551,8 +561,9 @@ static struct hotplug_slot *get_slot_from_name (const char *name)
  *
  * Returns 0 if successful, anything else for an error.
  */
-int pci_hp_register(struct hotplug_slot *slot, struct pci_bus *bus, int slot_nr,
-                        const char *name)
+int __pci_hp_register(struct hotplug_slot *slot, struct pci_bus *bus,
+                      int devnr, const char *name,
+                      struct module *owner, const char *mod_name)
 {
         int result;
         struct pci_slot *pci_slot;
@@ -567,14 +578,16 @@ int pci_hp_register(struct hotplug_slot *slot, struct pci_bus *bus, int slot_nr,
                 return -EINVAL;
         }
 
-        mutex_lock(&pci_hp_mutex);
+        slot->ops->owner = owner;
+        slot->ops->mod_name = mod_name;
 
+        mutex_lock(&pci_hp_mutex);
         /*
          * No problems if we call this interface from both ACPI_PCI_SLOT
          * driver and call it here again. If we've already created the
          * pci_slot, the interface will simply bump the refcount.
          */
-        pci_slot = pci_create_slot(bus, slot_nr, name, slot);
+        pci_slot = pci_create_slot(bus, devnr, name, slot);
         if (IS_ERR(pci_slot)) {
                 result = PTR_ERR(pci_slot);
                 goto out;
@@ -684,6 +697,6 @@ MODULE_LICENSE("GPL");
 module_param(debug, bool, 0644);
 MODULE_PARM_DESC(debug, "Debugging mode enabled or not");
 
-EXPORT_SYMBOL_GPL(pci_hp_register);
+EXPORT_SYMBOL_GPL(__pci_hp_register);
 EXPORT_SYMBOL_GPL(pci_hp_deregister);
 EXPORT_SYMBOL_GPL(pci_hp_change_slot_info);
diff --git a/drivers/pci/hotplug/pciehp.h b/drivers/pci/hotplug/pciehp.h
index 0a368547e633..e6cf096498be 100644
--- a/drivers/pci/hotplug/pciehp.h
+++ b/drivers/pci/hotplug/pciehp.h
@@ -81,7 +81,6 @@ struct slot {
         struct hpc_ops *hpc_ops;
         struct hotplug_slot *hotplug_slot;
         struct list_head        slot_list;
-        unsigned long last_emi_toggle;
         struct delayed_work work;       /* work for button event */
         struct mutex lock;
 };
@@ -203,8 +202,6 @@ struct hpc_ops {
         int (*set_attention_status)(struct slot *slot, u8 status);
         int (*get_latch_status)(struct slot *slot, u8 *status);
         int (*get_adapter_status)(struct slot *slot, u8 *status);
-        int (*get_emi_status)(struct slot *slot, u8 *status);
-        int (*toggle_emi)(struct slot *slot);
         int (*get_max_bus_speed)(struct slot *slot, enum pci_bus_speed *speed);
         int (*get_cur_bus_speed)(struct slot *slot, enum pci_bus_speed *speed);
         int (*get_max_lnk_width)(struct slot *slot, enum pcie_link_width *val);
diff --git a/drivers/pci/hotplug/pciehp_core.c b/drivers/pci/hotplug/pciehp_core.c
index fb254b2454de..2317557fdee6 100644
--- a/drivers/pci/hotplug/pciehp_core.c
+++ b/drivers/pci/hotplug/pciehp_core.c
@@ -73,7 +73,6 @@ static int get_max_bus_speed	(struct hotplug_slot *slot, enum pci_bus_speed *val
 static int get_cur_bus_speed    (struct hotplug_slot *slot, enum pci_bus_speed *value);
 
 static struct hotplug_slot_ops pciehp_hotplug_slot_ops = {
-        .owner =                THIS_MODULE,
         .set_attention_status = set_attention_status,
         .enable_slot =          enable_slot,
         .disable_slot =         disable_slot,
@@ -85,99 +84,6 @@ static struct hotplug_slot_ops pciehp_hotplug_slot_ops = {
         .get_cur_bus_speed =    get_cur_bus_speed,
 };
 
-/*
- * Check the status of the Electro Mechanical Interlock (EMI)
- */
-static int get_lock_status(struct hotplug_slot *hotplug_slot, u8 *value)
-{
-        struct slot *slot = hotplug_slot->private;
-        return (slot->hpc_ops->get_emi_status(slot, value));
-}
-
-/*
- * sysfs interface for the Electro Mechanical Interlock (EMI)
- * 1 == locked, 0 == unlocked
- */
-static ssize_t lock_read_file(struct hotplug_slot *slot, char *buf)
-{
-        int retval;
-        u8 value;
-
-        retval = get_lock_status(slot, &value);
-        if (retval)
-                goto lock_read_exit;
-        retval = sprintf (buf, "%d\n", value);
-
-lock_read_exit:
-        return retval;
-}
-
-/*
- * Change the status of the Electro Mechanical Interlock (EMI)
- * This is a toggle - in addition there must be at least 1 second
- * in between toggles.
- */
-static int set_lock_status(struct hotplug_slot *hotplug_slot, u8 status)
-{
-        struct slot *slot = hotplug_slot->private;
-        int retval;
-        u8 value;
-
-        mutex_lock(&slot->ctrl->crit_sect);
-
-        /* has it been >1 sec since our last toggle? */
-        if ((get_seconds() - slot->last_emi_toggle) < 1) {
-                mutex_unlock(&slot->ctrl->crit_sect);
-                return -EINVAL;
-        }
-
-        /* see what our current state is */
-        retval = get_lock_status(hotplug_slot, &value);
-        if (retval || (value == status))
-                goto set_lock_exit;
-
-        slot->hpc_ops->toggle_emi(slot);
-set_lock_exit:
-        mutex_unlock(&slot->ctrl->crit_sect);
-        return 0;
-}
-
-/*
- * sysfs interface which allows the user to toggle the Electro Mechanical
- * Interlock.  Valid values are either 0 or 1.  0 == unlock, 1 == lock
- */
-static ssize_t lock_write_file(struct hotplug_slot *hotplug_slot,
-                const char *buf, size_t count)
-{
-        struct slot *slot = hotplug_slot->private;
-        unsigned long llock;
-        u8 lock;
-        int retval = 0;
-
-        llock = simple_strtoul(buf, NULL, 10);
-        lock = (u8)(llock & 0xff);
-
-        switch (lock) {
-                case 0:
-                case 1:
-                        retval = set_lock_status(hotplug_slot, lock);
-                        break;
-                default:
-                        ctrl_err(slot->ctrl, "%d is an invalid lock value\n",
-                                 lock);
-                        retval = -EINVAL;
-        }
-        if (retval)
-                return retval;
-        return count;
-}
-
-static struct hotplug_slot_attribute hotplug_slot_attr_lock = {
-        .attr = {.name = "lock", .mode = S_IFREG | S_IRUGO | S_IWUSR},
-        .show = lock_read_file,
-        .store = lock_write_file
-};
-
 /**
  * release_slot - free up the memory used by a slot
  * @hotplug_slot: slot to free
@@ -236,17 +142,6 @@ static int init_slots(struct controller *ctrl)
                 get_attention_status(hotplug_slot, &info->attention_status);
                 get_latch_status(hotplug_slot, &info->latch_status);
                 get_adapter_status(hotplug_slot, &info->adapter_status);
-                /* create additional sysfs entries */
-                if (EMI(ctrl)) {
-                        retval = sysfs_create_file(&hotplug_slot->pci_slot->kobj,
-                                &hotplug_slot_attr_lock.attr);
-                        if (retval) {
-                                pci_hp_deregister(hotplug_slot);
-                                ctrl_err(ctrl, "Cannot create additional sysfs "
-                                         "entries\n");
-                                goto error_info;
-                        }
-                }
         }
 
         return 0;
@@ -261,13 +156,8 @@ error:
 static void cleanup_slots(struct controller *ctrl)
 {
         struct slot *slot;
-
-        list_for_each_entry(slot, &ctrl->slot_list, slot_list) {
-                if (EMI(ctrl))
-                        sysfs_remove_file(&slot->hotplug_slot->pci_slot->kobj,
-                                &hotplug_slot_attr_lock.attr);
+        list_for_each_entry(slot, &ctrl->slot_list, slot_list)
                 pci_hp_deregister(slot->hotplug_slot);
-        }
 }
 
 /*
diff --git a/drivers/pci/hotplug/pciehp_hpc.c b/drivers/pci/hotplug/pciehp_hpc.c
index 07bd32151146..52813257e5bf 100644
--- a/drivers/pci/hotplug/pciehp_hpc.c
+++ b/drivers/pci/hotplug/pciehp_hpc.c
@@ -422,35 +422,6 @@ static int hpc_query_power_fault(struct slot *slot)
         return !!(slot_status & PCI_EXP_SLTSTA_PFD);
 }
 
-static int hpc_get_emi_status(struct slot *slot, u8 *status)
-{
-        struct controller *ctrl = slot->ctrl;
-        u16 slot_status;
-        int retval;
-
-        retval = pciehp_readw(ctrl, PCI_EXP_SLTSTA, &slot_status);
-        if (retval) {
-                ctrl_err(ctrl, "Cannot check EMI status\n");
-                return retval;
-        }
-        *status = !!(slot_status & PCI_EXP_SLTSTA_EIS);
-        return retval;
-}
-
-static int hpc_toggle_emi(struct slot *slot)
-{
-        u16 slot_cmd;
-        u16 cmd_mask;
-        int rc;
-
-        slot_cmd = PCI_EXP_SLTCTL_EIC;
-        cmd_mask = PCI_EXP_SLTCTL_EIC;
-        rc = pcie_write_cmd(slot->ctrl, slot_cmd, cmd_mask);
-        slot->last_emi_toggle = get_seconds();
-
-        return rc;
-}
-
 static int hpc_set_attention_status(struct slot *slot, u8 value)
 {
         struct controller *ctrl = slot->ctrl;
@@ -874,8 +845,6 @@ static struct hpc_ops pciehp_hpc_ops = {
         .get_attention_status           = hpc_get_attention_status,
         .get_latch_status               = hpc_get_latch_status,
         .get_adapter_status             = hpc_get_adapter_status,
-        .get_emi_status                 = hpc_get_emi_status,
-        .toggle_emi                     = hpc_toggle_emi,
 
         .get_max_bus_speed              = hpc_get_max_lnk_speed,
         .get_cur_bus_speed              = hpc_get_cur_lnk_speed,
diff --git a/drivers/pci/hotplug/pcihp_skeleton.c b/drivers/pci/hotplug/pcihp_skeleton.c
index e3dd6cf9e89f..5175d9b26f0b 100644
--- a/drivers/pci/hotplug/pcihp_skeleton.c
+++ b/drivers/pci/hotplug/pcihp_skeleton.c
@@ -82,7 +82,6 @@ static int get_latch_status	(struct hotplug_slot *slot, u8 *value);
 static int get_adapter_status   (struct hotplug_slot *slot, u8 *value);
 
 static struct hotplug_slot_ops skel_hotplug_slot_ops = {
-        .owner =                THIS_MODULE,
         .enable_slot =          enable_slot,
         .disable_slot =         disable_slot,
         .set_attention_status = set_attention_status,
diff --git a/drivers/pci/hotplug/rpaphp_core.c b/drivers/pci/hotplug/rpaphp_core.c
index 95d02a08fdc7..c159223389ec 100644
--- a/drivers/pci/hotplug/rpaphp_core.c
+++ b/drivers/pci/hotplug/rpaphp_core.c
@@ -423,7 +423,6 @@ static int disable_slot(struct hotplug_slot *hotplug_slot)
 }
 
 struct hotplug_slot_ops rpaphp_hotplug_slot_ops = {
-        .owner = THIS_MODULE,
         .enable_slot = enable_slot,
         .disable_slot = disable_slot,
         .set_attention_status = set_attention_status,
diff --git a/drivers/pci/hotplug/sgi_hotplug.c b/drivers/pci/hotplug/sgi_hotplug.c
index 2d6da78fddb6..a4494d78e7c2 100644
--- a/drivers/pci/hotplug/sgi_hotplug.c
+++ b/drivers/pci/hotplug/sgi_hotplug.c
@@ -83,7 +83,6 @@ static int disable_slot(struct hotplug_slot *slot);
 static inline int get_power_status(struct hotplug_slot *slot, u8 *value);
 
 static struct hotplug_slot_ops sn_hotplug_slot_ops = {
-        .owner                  = THIS_MODULE,
         .enable_slot            = enable_slot,
         .disable_slot           = disable_slot,
         .get_power_status       = get_power_status,
diff --git a/drivers/pci/hotplug/shpchp_core.c b/drivers/pci/hotplug/shpchp_core.c
index fe8d149c2293..8a520a3d0f59 100644
--- a/drivers/pci/hotplug/shpchp_core.c
+++ b/drivers/pci/hotplug/shpchp_core.c
@@ -69,7 +69,6 @@ static int get_max_bus_speed	(struct hotplug_slot *slot, enum pci_bus_speed *val
 static int get_cur_bus_speed    (struct hotplug_slot *slot, enum pci_bus_speed *value);
 
 static struct hotplug_slot_ops shpchp_hotplug_slot_ops = {
-        .owner =                THIS_MODULE,
         .set_attention_status = set_attention_status,
         .enable_slot =          enable_slot,
         .disable_slot =         disable_slot,
diff --git a/drivers/pci/intel-iommu.c b/drivers/pci/intel-iommu.c
index cd389162735f..178853a07440 100644
--- a/drivers/pci/intel-iommu.c
+++ b/drivers/pci/intel-iommu.c
@@ -53,6 +53,8 @@
 
 #define DEFAULT_DOMAIN_ADDRESS_WIDTH 48
 
+#define MAX_AGAW_WIDTH 64
+
 #define DOMAIN_MAX_ADDR(gaw) ((((u64)1) << gaw) - 1)
 
 #define IOVA_PFN(addr)          ((addr) >> PAGE_SHIFT)
@@ -131,8 +133,6 @@ static inline void context_set_fault_enable(struct context_entry *context)
         context->lo &= (((u64)-1) << 2) | 1;
 }
 
-#define CONTEXT_TT_MULTI_LEVEL 0
-
 static inline void context_set_translation_type(struct context_entry *context,
                                                 unsigned long value)
 {
@@ -256,6 +256,7 @@ struct device_domain_info {
         u8 bus;                 /* PCI bus number */
         u8 devfn;               /* PCI devfn number */
         struct pci_dev *dev; /* it's NULL for PCIE-to-PCI bridge */
+        struct intel_iommu *iommu; /* IOMMU used by this device */
         struct dmar_domain *domain; /* pointer to domain */
 };
 
@@ -401,17 +402,13 @@ void free_iova_mem(struct iova *iova)
 
 static inline int width_to_agaw(int width);
 
-/* calculate agaw for each iommu.
- * "SAGAW" may be different across iommus, use a default agaw, and
- * get a supported less agaw for iommus that don't support the default agaw.
- */
-int iommu_calculate_agaw(struct intel_iommu *iommu)
+static int __iommu_calculate_agaw(struct intel_iommu *iommu, int max_gaw)
 {
         unsigned long sagaw;
         int agaw = -1;
 
         sagaw = cap_sagaw(iommu->cap);
-        for (agaw = width_to_agaw(DEFAULT_DOMAIN_ADDRESS_WIDTH);
+        for (agaw = width_to_agaw(max_gaw);
              agaw >= 0; agaw--) {
                 if (test_bit(agaw, &sagaw))
                         break;
@@ -420,6 +417,24 @@ int iommu_calculate_agaw(struct intel_iommu *iommu)
         return agaw;
 }
 
+/*
+ * Calculate max SAGAW for each iommu.
+ */
+int iommu_calculate_max_sagaw(struct intel_iommu *iommu)
+{
+        return __iommu_calculate_agaw(iommu, MAX_AGAW_WIDTH);
+}
+
+/*
+ * calculate agaw for each iommu.
+ * "SAGAW" may be different across iommus, use a default agaw, and
+ * get a supported less agaw for iommus that don't support the default agaw.
+ */
+int iommu_calculate_agaw(struct intel_iommu *iommu)
+{
+        return __iommu_calculate_agaw(iommu, DEFAULT_DOMAIN_ADDRESS_WIDTH);
+}
+
 /* in native case, each domain is related to only one iommu */
 static struct intel_iommu *domain_get_iommu(struct dmar_domain *domain)
 {
@@ -809,7 +824,7 @@ static int iommu_alloc_root_entry(struct intel_iommu *iommu)
 static void iommu_set_root_entry(struct intel_iommu *iommu)
 {
         void *addr;
-        u32 cmd, sts;
+        u32 sts;
         unsigned long flag;
 
         addr = iommu->root_entry;
@@ -817,12 +832,11 @@ static void iommu_set_root_entry(struct intel_iommu *iommu)
         spin_lock_irqsave(&iommu->register_lock, flag);
         dmar_writeq(iommu->reg + DMAR_RTADDR_REG, virt_to_phys(addr));
 
-        cmd = iommu->gcmd | DMA_GCMD_SRTP;
-        writel(cmd, iommu->reg + DMAR_GCMD_REG);
+        writel(iommu->gcmd | DMA_GCMD_SRTP, iommu->reg + DMAR_GCMD_REG);
 
         /* Make sure hardware complete it */
         IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
-                readl, (sts & DMA_GSTS_RTPS), sts);
+                      readl, (sts & DMA_GSTS_RTPS), sts);
 
         spin_unlock_irqrestore(&iommu->register_lock, flag);
 }
@@ -834,39 +848,25 @@ static void iommu_flush_write_buffer(struct intel_iommu *iommu)
 
         if (!rwbf_quirk && !cap_rwbf(iommu->cap))
                 return;
-        val = iommu->gcmd | DMA_GCMD_WBF;
 
         spin_lock_irqsave(&iommu->register_lock, flag);
-        writel(val, iommu->reg + DMAR_GCMD_REG);
+        writel(iommu->gcmd | DMA_GCMD_WBF, iommu->reg + DMAR_GCMD_REG);
 
         /* Make sure hardware complete it */
         IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
-                        readl, (!(val & DMA_GSTS_WBFS)), val);
+                      readl, (!(val & DMA_GSTS_WBFS)), val);
 
         spin_unlock_irqrestore(&iommu->register_lock, flag);
 }
 
 /* return value determine if we need a write buffer flush */
-static int __iommu_flush_context(struct intel_iommu *iommu,
-        u16 did, u16 source_id, u8 function_mask, u64 type,
-        int non_present_entry_flush)
+static void __iommu_flush_context(struct intel_iommu *iommu,
+                                  u16 did, u16 source_id, u8 function_mask,
+                                  u64 type)
 {
         u64 val = 0;
         unsigned long flag;
 
-        /*
-         * In the non-present entry flush case, if hardware doesn't cache
-         * non-present entry we do nothing and if hardware cache non-present
-         * entry, we flush entries of domain 0 (the domain id is used to cache
-         * any non-present entries)
-         */
-        if (non_present_entry_flush) {
-                if (!cap_caching_mode(iommu->cap))
-                        return 1;
-                else
-                        did = 0;
-        }
-
         switch (type) {
         case DMA_CCMD_GLOBAL_INVL:
                 val = DMA_CCMD_GLOBAL_INVL;
@@ -891,33 +891,16 @@ static int __iommu_flush_context(struct intel_iommu *iommu,
                 dmar_readq, (!(val & DMA_CCMD_ICC)), val);
 
         spin_unlock_irqrestore(&iommu->register_lock, flag);
-
-        /* flush context entry will implicitly flush write buffer */
-        return 0;
 }
 
 /* return value determine if we need a write buffer flush */
-static int __iommu_flush_iotlb(struct intel_iommu *iommu, u16 did,
-        u64 addr, unsigned int size_order, u64 type,
-        int non_present_entry_flush)
+static void __iommu_flush_iotlb(struct intel_iommu *iommu, u16 did,
+                                u64 addr, unsigned int size_order, u64 type)
 {
         int tlb_offset = ecap_iotlb_offset(iommu->ecap);
         u64 val = 0, val_iva = 0;
         unsigned long flag;
 
-        /*
-         * In the non-present entry flush case, if hardware doesn't cache
-         * non-present entry we do nothing and if hardware cache non-present
-         * entry, we flush entries of domain 0 (the domain id is used to cache
-         * any non-present entries)
-         */
-        if (non_present_entry_flush) {
-                if (!cap_caching_mode(iommu->cap))
-                        return 1;
-                else
-                        did = 0;
-        }
-
         switch (type) {
         case DMA_TLB_GLOBAL_FLUSH:
                 /* global flush doesn't need set IVA_REG */
@@ -965,37 +948,101 @@ static int __iommu_flush_iotlb(struct intel_iommu *iommu, u16 did,
                 pr_debug("IOMMU: tlb flush request %Lx, actual %Lx\n",
                         (unsigned long long)DMA_TLB_IIRG(type),
                         (unsigned long long)DMA_TLB_IAIG(val));
-        /* flush iotlb entry will implicitly flush write buffer */
-        return 0;
 }
 
-static int iommu_flush_iotlb_psi(struct intel_iommu *iommu, u16 did,
-        u64 addr, unsigned int pages, int non_present_entry_flush)
+static struct device_domain_info *iommu_support_dev_iotlb(
+        struct dmar_domain *domain, int segment, u8 bus, u8 devfn)
+{
+        int found = 0;
+        unsigned long flags;
+        struct device_domain_info *info;
+        struct intel_iommu *iommu = device_to_iommu(segment, bus, devfn);
+
+        if (!ecap_dev_iotlb_support(iommu->ecap))
+                return NULL;
+
+        if (!iommu->qi)
+                return NULL;
+
+        spin_lock_irqsave(&device_domain_lock, flags);
+        list_for_each_entry(info, &domain->devices, link)
+                if (info->bus == bus && info->devfn == devfn) {
+                        found = 1;
+                        break;
+                }
+        spin_unlock_irqrestore(&device_domain_lock, flags);
+
+        if (!found || !info->dev)
+                return NULL;
+
+        if (!pci_find_ext_capability(info->dev, PCI_EXT_CAP_ID_ATS))
+                return NULL;
+
+        if (!dmar_find_matched_atsr_unit(info->dev))
+                return NULL;
+
+        info->iommu = iommu;
+
+        return info;
+}
+
+static void iommu_enable_dev_iotlb(struct device_domain_info *info)
 {
-        unsigned int mask;
+        if (!info)
+                return;
+
+        pci_enable_ats(info->dev, VTD_PAGE_SHIFT);
+}
+
+static void iommu_disable_dev_iotlb(struct device_domain_info *info)
+{
+        if (!info->dev || !pci_ats_enabled(info->dev))
+                return;
+
+        pci_disable_ats(info->dev);
+}
+
+static void iommu_flush_dev_iotlb(struct dmar_domain *domain,
+                                  u64 addr, unsigned mask)
+{
+        u16 sid, qdep;
+        unsigned long flags;
+        struct device_domain_info *info;
+
+        spin_lock_irqsave(&device_domain_lock, flags);
+        list_for_each_entry(info, &domain->devices, link) {
+                if (!info->dev || !pci_ats_enabled(info->dev))
+                        continue;
+
+                sid = info->bus << 8 | info->devfn;
+                qdep = pci_ats_queue_depth(info->dev);
+                qi_flush_dev_iotlb(info->iommu, sid, qdep, addr, mask);
+        }
+        spin_unlock_irqrestore(&device_domain_lock, flags);
+}
+
+static void iommu_flush_iotlb_psi(struct intel_iommu *iommu, u16 did,
+                                  u64 addr, unsigned int pages)
+{
+        unsigned int mask = ilog2(__roundup_pow_of_two(pages));
 
         BUG_ON(addr & (~VTD_PAGE_MASK));
         BUG_ON(pages == 0);
 
-        /* Fallback to domain selective flush if no PSI support */
-        if (!cap_pgsel_inv(iommu->cap))
-                return iommu->flush.flush_iotlb(iommu, did, 0, 0,
-                                                DMA_TLB_DSI_FLUSH,
-                                                non_present_entry_flush);
-
         /*
+         * Fallback to domain selective flush if no PSI support or the size is
+         * too big.
          * PSI requires page size to be 2 ^ x, and the base address is naturally
          * aligned to the size
          */
-        mask = ilog2(__roundup_pow_of_two(pages));
-        /* Fallback to domain selective flush if size is too big */
-        if (mask > cap_max_amask_val(iommu->cap))
-                return iommu->flush.flush_iotlb(iommu, did, 0, 0,
-                        DMA_TLB_DSI_FLUSH, non_present_entry_flush);
-
-        return iommu->flush.flush_iotlb(iommu, did, addr, mask,
-                                        DMA_TLB_PSI_FLUSH,
-                                        non_present_entry_flush);
+        if (!cap_pgsel_inv(iommu->cap) || mask > cap_max_amask_val(iommu->cap))
+                iommu->flush.flush_iotlb(iommu, did, 0, 0,
+                                                DMA_TLB_DSI_FLUSH);
+        else
+                iommu->flush.flush_iotlb(iommu, did, addr, mask,
+                                                DMA_TLB_PSI_FLUSH);
+        if (did)
+                iommu_flush_dev_iotlb(iommu->domains[did], addr, mask);
 }
 
 static void iommu_disable_protect_mem_regions(struct intel_iommu *iommu)
@@ -1021,13 +1068,13 @@ static int iommu_enable_translation(struct intel_iommu *iommu)
         unsigned long flags;
 
         spin_lock_irqsave(&iommu->register_lock, flags);
-        writel(iommu->gcmd|DMA_GCMD_TE, iommu->reg + DMAR_GCMD_REG);
+        iommu->gcmd |= DMA_GCMD_TE;
+        writel(iommu->gcmd, iommu->reg + DMAR_GCMD_REG);
 
         /* Make sure hardware complete it */
         IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
-                readl, (sts & DMA_GSTS_TES), sts);
+                      readl, (sts & DMA_GSTS_TES), sts);
 
-        iommu->gcmd |= DMA_GCMD_TE;
         spin_unlock_irqrestore(&iommu->register_lock, flags);
         return 0;
 }
@@ -1043,7 +1090,7 @@ static int iommu_disable_translation(struct intel_iommu *iommu)
 
         /* Make sure hardware complete it */
         IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
-                readl, (!(sts & DMA_GSTS_TES)), sts);
+                      readl, (!(sts & DMA_GSTS_TES)), sts);
 
         spin_unlock_irqrestore(&iommu->register_lock, flag);
         return 0;
@@ -1325,8 +1372,8 @@ static void domain_exit(struct dmar_domain *domain)
         free_domain_mem(domain);
 }
 
-static int domain_context_mapping_one(struct dmar_domain *domain,
-                                      int segment, u8 bus, u8 devfn)
+static int domain_context_mapping_one(struct dmar_domain *domain, int segment,
+                                 u8 bus, u8 devfn, int translation)
 {
         struct context_entry *context;
         unsigned long flags;
@@ -1336,10 +1383,14 @@ static int domain_context_mapping_one(struct dmar_domain *domain,
         unsigned long ndomains;
         int id;
         int agaw;
+        struct device_domain_info *info = NULL;
 
         pr_debug("Set context mapping for %02x:%02x.%d\n",
                 bus, PCI_SLOT(devfn), PCI_FUNC(devfn));
+
         BUG_ON(!domain->pgd);
+        BUG_ON(translation != CONTEXT_TT_PASS_THROUGH &&
+               translation != CONTEXT_TT_MULTI_LEVEL);
 
         iommu = device_to_iommu(segment, bus, devfn);
         if (!iommu)
@@ -1399,21 +1450,44 @@ static int domain_context_mapping_one(struct dmar_domain *domain,
         }
 
         context_set_domain_id(context, id);
-        context_set_address_width(context, iommu->agaw);
-        context_set_address_root(context, virt_to_phys(pgd));
-        context_set_translation_type(context, CONTEXT_TT_MULTI_LEVEL);
+
+        if (translation != CONTEXT_TT_PASS_THROUGH) {
+                info = iommu_support_dev_iotlb(domain, segment, bus, devfn);
+                translation = info ? CONTEXT_TT_DEV_IOTLB :
+                                     CONTEXT_TT_MULTI_LEVEL;
+        }
+        /*
+         * In pass through mode, AW must be programmed to indicate the largest
+         * AGAW value supported by hardware. And ASR is ignored by hardware.
+         */
+        if (unlikely(translation == CONTEXT_TT_PASS_THROUGH))
+                context_set_address_width(context, iommu->msagaw);
+        else {
+                context_set_address_root(context, virt_to_phys(pgd));
+                context_set_address_width(context, iommu->agaw);
+        }
+
+        context_set_translation_type(context, translation);
         context_set_fault_enable(context);
         context_set_present(context);
         domain_flush_cache(domain, context, sizeof(*context));
 
-        /* it's a non-present to present mapping */
-        if (iommu->flush.flush_context(iommu, domain->id,
-                (((u16)bus) << 8) | devfn, DMA_CCMD_MASK_NOBIT,
-                DMA_CCMD_DEVICE_INVL, 1))
+        /*
+         * It's a non-present to present mapping. If hardware doesn't cache
+         * non-present entry we only need to flush the write-buffer. If the
+         * _does_ cache non-present entries, then it does so in the special
+         * domain #0, which we have to flush:
+         */
+        if (cap_caching_mode(iommu->cap)) {
+                iommu->flush.flush_context(iommu, 0,
+                                           (((u16)bus) << 8) | devfn,
+                                           DMA_CCMD_MASK_NOBIT,
+                                           DMA_CCMD_DEVICE_INVL);
+                iommu->flush.flush_iotlb(iommu, 0, 0, 0, DMA_TLB_DSI_FLUSH);
+        } else {
                 iommu_flush_write_buffer(iommu);
-        else
-                iommu->flush.flush_iotlb(iommu, 0, 0, 0, DMA_TLB_DSI_FLUSH, 0);
-
+        }
+        iommu_enable_dev_iotlb(info);
         spin_unlock_irqrestore(&iommu->lock, flags);
 
         spin_lock_irqsave(&domain->iommu_lock, flags);
@@ -1426,13 +1500,15 @@ static int domain_context_mapping_one(struct dmar_domain *domain,
 }
 
 static int
-domain_context_mapping(struct dmar_domain *domain, struct pci_dev *pdev)
+domain_context_mapping(struct dmar_domain *domain, struct pci_dev *pdev,
+                        int translation)
 {
         int ret;
         struct pci_dev *tmp, *parent;
 
         ret = domain_context_mapping_one(domain, pci_domain_nr(pdev->bus),
-                                         pdev->bus->number, pdev->devfn);
+                                         pdev->bus->number, pdev->devfn,
+                                         translation);
         if (ret)
                 return ret;
 
@@ -1446,7 +1522,7 @@ domain_context_mapping(struct dmar_domain *domain, struct pci_dev *pdev)
                 ret = domain_context_mapping_one(domain,
                                                  pci_domain_nr(parent->bus),
                                                  parent->bus->number,
-                                                 parent->devfn);
+                                                 parent->devfn, translation);
                 if (ret)
                         return ret;
                 parent = parent->bus->self;
@@ -1454,12 +1530,14 @@ domain_context_mapping(struct dmar_domain *domain, struct pci_dev *pdev)
         if (tmp->is_pcie) /* this is a PCIE-to-PCI bridge */
                 return domain_context_mapping_one(domain,
                                         pci_domain_nr(tmp->subordinate),
-                                        tmp->subordinate->number, 0);
+                                        tmp->subordinate->number, 0,
+                                        translation);
         else /* this is a legacy PCI bridge */
                 return domain_context_mapping_one(domain,
                                                   pci_domain_nr(tmp->bus),
                                                   tmp->bus->number,
-                                                  tmp->devfn);
+                                                  tmp->devfn,
+                                                  translation);
 }
 
 static int domain_context_mapped(struct pci_dev *pdev)
@@ -1540,9 +1618,8 @@ static void iommu_detach_dev(struct intel_iommu *iommu, u8 bus, u8 devfn)
 
         clear_context_table(iommu, bus, devfn);
         iommu->flush.flush_context(iommu, 0, 0, 0,
-                                           DMA_CCMD_GLOBAL_INVL, 0);
-        iommu->flush.flush_iotlb(iommu, 0, 0, 0,
-                                         DMA_TLB_GLOBAL_FLUSH, 0);
+                                           DMA_CCMD_GLOBAL_INVL);
+        iommu->flush.flush_iotlb(iommu, 0, 0, 0, DMA_TLB_GLOBAL_FLUSH);
 }
 
 static void domain_remove_dev_info(struct dmar_domain *domain)
@@ -1561,6 +1638,7 @@ static void domain_remove_dev_info(struct dmar_domain *domain)
                         info->dev->dev.archdata.iommu = NULL;
                 spin_unlock_irqrestore(&device_domain_lock, flags);
 
+                iommu_disable_dev_iotlb(info);
                 iommu = device_to_iommu(info->segment, info->bus, info->devfn);
                 iommu_detach_dev(iommu, info->bus, info->devfn);
                 free_devinfo_mem(info);
@@ -1756,7 +1834,7 @@ static int iommu_prepare_identity_map(struct pci_dev *pdev,
                 goto error;
 
         /* context entry init */
-        ret = domain_context_mapping(domain, pdev);
+        ret = domain_context_mapping(domain, pdev, CONTEXT_TT_MULTI_LEVEL);
         if (!ret)
                 return 0;
 error:
@@ -1857,6 +1935,23 @@ static inline void iommu_prepare_isa(void)
 }
 #endif /* !CONFIG_DMAR_FLPY_WA */
 
+/* Initialize each context entry as pass through.*/
+static int __init init_context_pass_through(void)
+{
+        struct pci_dev *pdev = NULL;
+        struct dmar_domain *domain;
+        int ret;
+
+        for_each_pci_dev(pdev) {
+                domain = get_domain_for_dev(pdev, DEFAULT_DOMAIN_ADDRESS_WIDTH);
+                ret = domain_context_mapping(domain, pdev,
+                                             CONTEXT_TT_PASS_THROUGH);
+                if (ret)
+                        return ret;
+        }
+        return 0;
+}
+
 static int __init init_dmars(void)
 {
         struct dmar_drhd_unit *drhd;
@@ -1864,6 +1959,7 @@ static int __init init_dmars(void)
         struct pci_dev *pdev;
         struct intel_iommu *iommu;
         int i, ret;
+        int pass_through = 1;
 
         /*
          * for each drhd
@@ -1917,7 +2013,15 @@ static int __init init_dmars(void)
                         printk(KERN_ERR "IOMMU: allocate root entry failed\n");
                         goto error;
                 }
+                if (!ecap_pass_through(iommu->ecap))
+                        pass_through = 0;
         }
+        if (iommu_pass_through)
+                if (!pass_through) {
+                        printk(KERN_INFO
+                               "Pass Through is not supported by hardware.\n");
+                        iommu_pass_through = 0;
+                }
 
         /*
          * Start from the sane iommu hardware state.
@@ -1973,35 +2077,56 @@ static int __init init_dmars(void)
         }
 
         /*
-         * For each rmrr
-         *   for each dev attached to rmrr
-         *   do
-         *     locate drhd for dev, alloc domain for dev
-         *     allocate free domain
-         *     allocate page table entries for rmrr
-         *     if context not allocated for bus
-         *           allocate and init context
-         *           set present in root table for this bus
-         *     init context with domain, translation etc
-         *    endfor
-         * endfor
+         * If pass through is set and enabled, context entries of all pci
+         * devices are intialized by pass through translation type.
          */
-        for_each_rmrr_units(rmrr) {
-                for (i = 0; i < rmrr->devices_cnt; i++) {
-                        pdev = rmrr->devices[i];
-                        /* some BIOS lists non-exist devices in DMAR table */
-                        if (!pdev)
-                                continue;
-                        ret = iommu_prepare_rmrr_dev(rmrr, pdev);
-                        if (ret)
-                                printk(KERN_ERR
-                                 "IOMMU: mapping reserved region failed\n");
+        if (iommu_pass_through) {
+                ret = init_context_pass_through();
+                if (ret) {
+                        printk(KERN_ERR "IOMMU: Pass through init failed.\n");
+                        iommu_pass_through = 0;
                 }
         }
 
-        iommu_prepare_gfx_mapping();
+        /*
+         * If pass through is not set or not enabled, setup context entries for
+         * identity mappings for rmrr, gfx, and isa.
+         */
+        if (!iommu_pass_through) {
+                /*
+                 * For each rmrr
+                 *   for each dev attached to rmrr
+                 *   do
+                 *     locate drhd for dev, alloc domain for dev
+                 *     allocate free domain
+                 *     allocate page table entries for rmrr
+                 *     if context not allocated for bus
+                 *           allocate and init context
+                 *           set present in root table for this bus
+                 *     init context with domain, translation etc
+                 *    endfor
+                 * endfor
+                 */
+                for_each_rmrr_units(rmrr) {
+                        for (i = 0; i < rmrr->devices_cnt; i++) {
+                                pdev = rmrr->devices[i];
+                                /*
+                                 * some BIOS lists non-exist devices in DMAR
+                                 * table.
+                                 */
+                                if (!pdev)
+                                        continue;
+                                ret = iommu_prepare_rmrr_dev(rmrr, pdev);
+                                if (ret)
+                                        printk(KERN_ERR
+                                 "IOMMU: mapping reserved region failed\n");
+                        }
+                }
+
+                iommu_prepare_gfx_mapping();
 
-        iommu_prepare_isa();
+                iommu_prepare_isa();
+        }
 
         /*
          * for each drhd
@@ -2023,10 +2148,8 @@ static int __init init_dmars(void)
 
                 iommu_set_root_entry(iommu);
 
-                iommu->flush.flush_context(iommu, 0, 0, 0, DMA_CCMD_GLOBAL_INVL,
-                                           0);
-                iommu->flush.flush_iotlb(iommu, 0, 0, 0, DMA_TLB_GLOBAL_FLUSH,
-                                         0);
+                iommu->flush.flush_context(iommu, 0, 0, 0, DMA_CCMD_GLOBAL_INVL);
+                iommu->flush.flush_iotlb(iommu, 0, 0, 0, DMA_TLB_GLOBAL_FLUSH);
                 iommu_disable_protect_mem_regions(iommu);
 
                 ret = iommu_enable_translation(iommu);
@@ -2112,7 +2235,8 @@ get_valid_domain_for_dev(struct pci_dev *pdev)
 
         /* make sure context mapping is ok */
         if (unlikely(!domain_context_mapped(pdev))) {
-                ret = domain_context_mapping(domain, pdev);
+                ret = domain_context_mapping(domain, pdev,
+                                             CONTEXT_TT_MULTI_LEVEL);
                 if (ret) {
                         printk(KERN_ERR
                                 "Domain context map for %s failed",
@@ -2173,10 +2297,11 @@ static dma_addr_t __intel_map_single(struct device *hwdev, phys_addr_t paddr,
         if (ret)
                 goto error;
 
-        /* it's a non-present to present mapping */
-        ret = iommu_flush_iotlb_psi(iommu, domain->id,
-                        start_paddr, size >> VTD_PAGE_SHIFT, 1);
-        if (ret)
+        /* it's a non-present to present mapping. Only flush if caching mode */
+        if (cap_caching_mode(iommu->cap))
+                iommu_flush_iotlb_psi(iommu, 0, start_paddr,
+                                      size >> VTD_PAGE_SHIFT);
+        else
                 iommu_flush_write_buffer(iommu);
 
         return start_paddr + ((u64)paddr & (~PAGE_MASK));
@@ -2210,15 +2335,22 @@ static void flush_unmaps(void)
                 if (!iommu)
                         continue;
 
-                if (deferred_flush[i].next) {
-                        iommu->flush.flush_iotlb(iommu, 0, 0, 0,
-                                                 DMA_TLB_GLOBAL_FLUSH, 0);
-                        for (j = 0; j < deferred_flush[i].next; j++) {
-                                __free_iova(&deferred_flush[i].domain[j]->iovad,
-                                                deferred_flush[i].iova[j]);
-                        }
-                        deferred_flush[i].next = 0;
+                if (!deferred_flush[i].next)
+                        continue;
+
+                iommu->flush.flush_iotlb(iommu, 0, 0, 0,
+                                         DMA_TLB_GLOBAL_FLUSH);
+                for (j = 0; j < deferred_flush[i].next; j++) {
+                        unsigned long mask;
+                        struct iova *iova = deferred_flush[i].iova[j];
+
+                        mask = (iova->pfn_hi - iova->pfn_lo + 1) << PAGE_SHIFT;
+                        mask = ilog2(mask >> VTD_PAGE_SHIFT);
+                        iommu_flush_dev_iotlb(deferred_flush[i].domain[j],
+                                        iova->pfn_lo << PAGE_SHIFT, mask);
+                        __free_iova(&deferred_flush[i].domain[j]->iovad, iova);
                 }
+                deferred_flush[i].next = 0;
         }
 
         list_size = 0;
@@ -2291,9 +2423,8 @@ static void intel_unmap_page(struct device *dev, dma_addr_t dev_addr,
         /* free page tables */
         dma_pte_free_pagetable(domain, start_addr, start_addr + size);
         if (intel_iommu_strict) {
-                if (iommu_flush_iotlb_psi(iommu,
-                        domain->id, start_addr, size >> VTD_PAGE_SHIFT, 0))
-                        iommu_flush_write_buffer(iommu);
+                iommu_flush_iotlb_psi(iommu, domain->id, start_addr,
+                                      size >> VTD_PAGE_SHIFT);
                 /* free iova */
                 __free_iova(&domain->iovad, iova);
         } else {
@@ -2384,9 +2515,8 @@ static void intel_unmap_sg(struct device *hwdev, struct scatterlist *sglist,
         /* free page tables */
         dma_pte_free_pagetable(domain, start_addr, start_addr + size);
 
-        if (iommu_flush_iotlb_psi(iommu, domain->id, start_addr,
-                        size >> VTD_PAGE_SHIFT, 0))
-                iommu_flush_write_buffer(iommu);
+        iommu_flush_iotlb_psi(iommu, domain->id, start_addr,
+                              size >> VTD_PAGE_SHIFT);
 
         /* free iova */
         __free_iova(&domain->iovad, iova);
@@ -2478,10 +2608,13 @@ static int intel_map_sg(struct device *hwdev, struct scatterlist *sglist, int ne
                 offset += size;
         }
 
-        /* it's a non-present to present mapping */
-        if (iommu_flush_iotlb_psi(iommu, domain->id,
-                        start_addr, offset >> VTD_PAGE_SHIFT, 1))
+        /* it's a non-present to present mapping. Only flush if caching mode */
+        if (cap_caching_mode(iommu->cap))
+                iommu_flush_iotlb_psi(iommu, 0, start_addr,
+                                      offset >> VTD_PAGE_SHIFT);
+        else
                 iommu_flush_write_buffer(iommu);
+
         return nelems;
 }
 
@@ -2640,9 +2773,9 @@ static int init_iommu_hw(void)
                 iommu_set_root_entry(iommu);
 
                 iommu->flush.flush_context(iommu, 0, 0, 0,
-                                                DMA_CCMD_GLOBAL_INVL, 0);
+                                           DMA_CCMD_GLOBAL_INVL);
                 iommu->flush.flush_iotlb(iommu, 0, 0, 0,
-                                                DMA_TLB_GLOBAL_FLUSH, 0);
+                                         DMA_TLB_GLOBAL_FLUSH);
                 iommu_disable_protect_mem_regions(iommu);
                 iommu_enable_translation(iommu);
         }
@@ -2657,9 +2790,9 @@ static void iommu_flush_all(void)
 
         for_each_active_iommu(iommu, drhd) {
                 iommu->flush.flush_context(iommu, 0, 0, 0,
-                                                DMA_CCMD_GLOBAL_INVL, 0);
+                                           DMA_CCMD_GLOBAL_INVL);
                 iommu->flush.flush_iotlb(iommu, 0, 0, 0,
-                                                DMA_TLB_GLOBAL_FLUSH, 0);
+                                         DMA_TLB_GLOBAL_FLUSH);
         }
 }
 
@@ -2782,7 +2915,7 @@ int __init intel_iommu_init(void)
          * Check the need for DMA-remapping initialization now.
          * Above initialization will also be used by Interrupt-remapping.
          */
-        if (no_iommu || swiotlb || dmar_disabled)
+        if (no_iommu || (swiotlb && !iommu_pass_through) || dmar_disabled)
                 return -ENODEV;
 
         iommu_init_mempool();
@@ -2802,7 +2935,15 @@ int __init intel_iommu_init(void)
 
         init_timer(&unmap_timer);
         force_iommu = 1;
-        dma_ops = &intel_dma_ops;
+
+        if (!iommu_pass_through) {
+                printk(KERN_INFO
+                       "Multi-level page-table translation for DMAR.\n");
+                dma_ops = &intel_dma_ops;
+        } else
+                printk(KERN_INFO
+                       "DMAR: Pass through translation for DMAR.\n");
+
         init_iommu_sysfs();
 
         register_iommu(&intel_iommu_ops);
@@ -2888,6 +3029,7 @@ static void vm_domain_remove_one_dev_info(struct dmar_domain *domain,
                                 info->dev->dev.archdata.iommu = NULL;
                         spin_unlock_irqrestore(&device_domain_lock, flags);
 
+                        iommu_disable_dev_iotlb(info);
                         iommu_detach_dev(iommu, info->bus, info->devfn);
                         iommu_detach_dependent_devices(iommu, pdev);
                         free_devinfo_mem(info);
@@ -2938,6 +3080,7 @@ static void vm_domain_remove_all_dev_info(struct dmar_domain *domain)
 
                 spin_unlock_irqrestore(&device_domain_lock, flags1);
 
+                iommu_disable_dev_iotlb(info);
                 iommu = device_to_iommu(info->segment, info->bus, info->devfn);
                 iommu_detach_dev(iommu, info->bus, info->devfn);
                 iommu_detach_dependent_devices(iommu, info->dev);
@@ -3142,11 +3285,11 @@ static int intel_iommu_attach_device(struct iommu_domain *domain,
                 return -EFAULT;
         }
 
-        ret = domain_context_mapping(dmar_domain, pdev);
+        ret = vm_domain_add_dev_info(dmar_domain, pdev);
         if (ret)
                 return ret;
 
-        ret = vm_domain_add_dev_info(dmar_domain, pdev);
+        ret = domain_context_mapping(dmar_domain, pdev, CONTEXT_TT_MULTI_LEVEL);
         return ret;
 }
 
diff --git a/drivers/pci/intr_remapping.c b/drivers/pci/intr_remapping.c
index 3a0cb0bb0593..1e83c8c5f985 100644
--- a/drivers/pci/intr_remapping.c
+++ b/drivers/pci/intr_remapping.c
@@ -409,7 +409,7 @@ int free_irte(int irq)
 static void iommu_set_intr_remapping(struct intel_iommu *iommu, int mode)
 {
         u64 addr;
-        u32 cmd, sts;
+        u32 sts;
         unsigned long flags;
 
         addr = virt_to_phys((void *)iommu->ir_table->base);
@@ -420,9 +420,8 @@ static void iommu_set_intr_remapping(struct intel_iommu *iommu, int mode)
                     (addr) | IR_X2APIC_MODE(mode) | INTR_REMAP_TABLE_REG_SIZE);
 
         /* Set interrupt-remapping table pointer */
-        cmd = iommu->gcmd | DMA_GCMD_SIRTP;
         iommu->gcmd |= DMA_GCMD_SIRTP;
-        writel(cmd, iommu->reg + DMAR_GCMD_REG);
+        writel(iommu->gcmd, iommu->reg + DMAR_GCMD_REG);
 
         IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
                       readl, (sts & DMA_GSTS_IRTPS), sts);
@@ -437,9 +436,8 @@ static void iommu_set_intr_remapping(struct intel_iommu *iommu, int mode)
         spin_lock_irqsave(&iommu->register_lock, flags);
 
         /* Enable interrupt-remapping */
-        cmd = iommu->gcmd | DMA_GCMD_IRE;
         iommu->gcmd |= DMA_GCMD_IRE;
-        writel(cmd, iommu->reg + DMAR_GCMD_REG);
+        writel(iommu->gcmd, iommu->reg + DMAR_GCMD_REG);
 
         IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
                       readl, (sts & DMA_GSTS_IRES), sts);
diff --git a/drivers/pci/iov.c b/drivers/pci/iov.c
index b497daab3d4a..e3a87210e947 100644
--- a/drivers/pci/iov.c
+++ b/drivers/pci/iov.c
@@ -5,6 +5,7 @@
  *
  * PCI Express I/O Virtualization (IOV) support.
  *   Single Root IOV 1.0
+ *   Address Translation Service 1.0
  */
 
 #include <linux/pci.h>
@@ -110,7 +111,7 @@ static int virtfn_add(struct pci_dev *dev, int id, int reset)
         }
 
         if (reset)
-                pci_execute_reset_function(virtfn);
+                __pci_reset_function(virtfn);
 
         pci_device_add(virtfn, virtfn->bus);
         mutex_unlock(&iov->dev->sriov->lock);
@@ -164,7 +165,7 @@ static void virtfn_remove(struct pci_dev *dev, int id, int reset)
 
         if (reset) {
                 device_release_driver(&virtfn->dev);
-                pci_execute_reset_function(virtfn);
+                __pci_reset_function(virtfn);
         }
 
         sprintf(buf, "virtfn%u", id);
@@ -487,13 +488,15 @@ found:
         iov->self = dev;
         pci_read_config_dword(dev, pos + PCI_SRIOV_CAP, &iov->cap);
         pci_read_config_byte(dev, pos + PCI_SRIOV_FUNC_LINK, &iov->link);
+        if (dev->pcie_type == PCI_EXP_TYPE_RC_END)
+                iov->link = PCI_DEVFN(PCI_SLOT(dev->devfn), iov->link);
 
         if (pdev)
                 iov->dev = pci_dev_get(pdev);
-        else {
+        else
                 iov->dev = dev;
-                mutex_init(&iov->lock);
-        }
+
+        mutex_init(&iov->lock);
 
         dev->sriov = iov;
         dev->is_physfn = 1;
@@ -513,11 +516,11 @@ static void sriov_release(struct pci_dev *dev)
 {
         BUG_ON(dev->sriov->nr_virtfn);
 
-        if (dev == dev->sriov->dev)
-                mutex_destroy(&dev->sriov->lock);
-        else
+        if (dev != dev->sriov->dev)
                 pci_dev_put(dev->sriov->dev);
 
+        mutex_destroy(&dev->sriov->lock);
+
         kfree(dev->sriov);
         dev->sriov = NULL;
 }
@@ -679,3 +682,145 @@ irqreturn_t pci_sriov_migration(struct pci_dev *dev)
         return sriov_migration(dev) ? IRQ_HANDLED : IRQ_NONE;
 }
 EXPORT_SYMBOL_GPL(pci_sriov_migration);
+
+static int ats_alloc_one(struct pci_dev *dev, int ps)
+{
+        int pos;
+        u16 cap;
+        struct pci_ats *ats;
+
+        pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ATS);
+        if (!pos)
+                return -ENODEV;
+
+        ats = kzalloc(sizeof(*ats), GFP_KERNEL);
+        if (!ats)
+                return -ENOMEM;
+
+        ats->pos = pos;
+        ats->stu = ps;
+        pci_read_config_word(dev, pos + PCI_ATS_CAP, &cap);
+        ats->qdep = PCI_ATS_CAP_QDEP(cap) ? PCI_ATS_CAP_QDEP(cap) :
+                                            PCI_ATS_MAX_QDEP;
+        dev->ats = ats;
+
+        return 0;
+}
+
+static void ats_free_one(struct pci_dev *dev)
+{
+        kfree(dev->ats);
+        dev->ats = NULL;
+}
+
+/**
+ * pci_enable_ats - enable the ATS capability
+ * @dev: the PCI device
+ * @ps: the IOMMU page shift
+ *
+ * Returns 0 on success, or negative on failure.
+ */
+int pci_enable_ats(struct pci_dev *dev, int ps)
+{
+        int rc;
+        u16 ctrl;
+
+        BUG_ON(dev->ats && dev->ats->is_enabled);
+
+        if (ps < PCI_ATS_MIN_STU)
+                return -EINVAL;
+
+        if (dev->is_physfn || dev->is_virtfn) {
+                struct pci_dev *pdev = dev->is_physfn ? dev : dev->physfn;
+
+                mutex_lock(&pdev->sriov->lock);
+                if (pdev->ats)
+                        rc = pdev->ats->stu == ps ? 0 : -EINVAL;
+                else
+                        rc = ats_alloc_one(pdev, ps);
+
+                if (!rc)
+                        pdev->ats->ref_cnt++;
+                mutex_unlock(&pdev->sriov->lock);
+                if (rc)
+                        return rc;
+        }
+
+        if (!dev->is_physfn) {
+                rc = ats_alloc_one(dev, ps);
+                if (rc)
+                        return rc;
+        }
+
+        ctrl = PCI_ATS_CTRL_ENABLE;
+        if (!dev->is_virtfn)
+                ctrl |= PCI_ATS_CTRL_STU(ps - PCI_ATS_MIN_STU);
+        pci_write_config_word(dev, dev->ats->pos + PCI_ATS_CTRL, ctrl);
+
+        dev->ats->is_enabled = 1;
+
+        return 0;
+}
+
+/**
+ * pci_disable_ats - disable the ATS capability
+ * @dev: the PCI device
+ */
+void pci_disable_ats(struct pci_dev *dev)
+{
+        u16 ctrl;
+
+        BUG_ON(!dev->ats || !dev->ats->is_enabled);
+
+        pci_read_config_word(dev, dev->ats->pos + PCI_ATS_CTRL, &ctrl);
+        ctrl &= ~PCI_ATS_CTRL_ENABLE;
+        pci_write_config_word(dev, dev->ats->pos + PCI_ATS_CTRL, ctrl);
+
+        dev->ats->is_enabled = 0;
+
+        if (dev->is_physfn || dev->is_virtfn) {
+                struct pci_dev *pdev = dev->is_physfn ? dev : dev->physfn;
+
+                mutex_lock(&pdev->sriov->lock);
+                pdev->ats->ref_cnt--;
+                if (!pdev->ats->ref_cnt)
+                        ats_free_one(pdev);
+                mutex_unlock(&pdev->sriov->lock);
+        }
+
+        if (!dev->is_physfn)
+                ats_free_one(dev);
+}
+
+/**
+ * pci_ats_queue_depth - query the ATS Invalidate Queue Depth
+ * @dev: the PCI device
+ *
+ * Returns the queue depth on success, or negative on failure.
+ *
+ * The ATS spec uses 0 in the Invalidate Queue Depth field to
+ * indicate that the function can accept 32 Invalidate Request.
+ * But here we use the `real' values (i.e. 1~32) for the Queue
+ * Depth; and 0 indicates the function shares the Queue with
+ * other functions (doesn't exclusively own a Queue).
+ */
+int pci_ats_queue_depth(struct pci_dev *dev)
+{
+        int pos;
+        u16 cap;
+
+        if (dev->is_virtfn)
+                return 0;
+
+        if (dev->ats)
+                return dev->ats->qdep;
+
+        pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ATS);
+        if (!pos)
+                return -ENODEV;
+
+        pci_read_config_word(dev, pos + PCI_ATS_CAP, &cap);
+
+        return PCI_ATS_CAP_QDEP(cap) ? PCI_ATS_CAP_QDEP(cap) :
+                                       PCI_ATS_MAX_QDEP;
+}
diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index 362773247fbf..d9f06fbfa0bf 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -75,22 +75,17 @@ void arch_teardown_msi_irqs(struct pci_dev *dev)
 }
 #endif
 
-static void __msi_set_enable(struct pci_dev *dev, int pos, int enable)
+static void msi_set_enable(struct pci_dev *dev, int pos, int enable)
 {
         u16 control;
 
-        if (pos) {
-                pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &control);
-                control &= ~PCI_MSI_FLAGS_ENABLE;
-                if (enable)
-                        control |= PCI_MSI_FLAGS_ENABLE;
-                pci_write_config_word(dev, pos + PCI_MSI_FLAGS, control);
-        }
-}
+        BUG_ON(!pos);
 
-static void msi_set_enable(struct pci_dev *dev, int enable)
-{
-        __msi_set_enable(dev, pci_find_capability(dev, PCI_CAP_ID_MSI), enable);
+        pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &control);
+        control &= ~PCI_MSI_FLAGS_ENABLE;
+        if (enable)
+                control |= PCI_MSI_FLAGS_ENABLE;
+        pci_write_config_word(dev, pos + PCI_MSI_FLAGS, control);
 }
 
 static void msix_set_enable(struct pci_dev *dev, int enable)
@@ -131,9 +126,6 @@ static inline __attribute_const__ u32 msi_enabled_mask(u16 control)
  * mask all MSI interrupts by clearing the MSI enable bit does not work
  * reliably as devices without an INTx disable bit will then generate a
  * level IRQ which will never be cleared.
- *
- * Returns 1 if it succeeded in masking the interrupt and 0 if the device
- * doesn't support MSI masking.
  */
 static void msi_mask_irq(struct msi_desc *desc, u32 mask, u32 flag)
 {
@@ -303,7 +295,7 @@ static void __pci_restore_msi_state(struct pci_dev *dev)
         pos = entry->msi_attrib.pos;
 
         pci_intx_for_msi(dev, 0);
-        msi_set_enable(dev, 0);
+        msi_set_enable(dev, pos, 0);
         write_msi_msg(dev->irq, &entry->msg);
 
         pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &control);
@@ -321,22 +313,22 @@ static void __pci_restore_msix_state(struct pci_dev *dev)
 
         if (!dev->msix_enabled)
                 return;
+        BUG_ON(list_empty(&dev->msi_list));
+        entry = list_entry(dev->msi_list.next, struct msi_desc, list);
+        pos = entry->msi_attrib.pos;
+        pci_read_config_word(dev, pos + PCI_MSIX_FLAGS, &control);
 
         /* route the table */
         pci_intx_for_msi(dev, 0);
-        msix_set_enable(dev, 0);
+        control |= PCI_MSIX_FLAGS_ENABLE | PCI_MSIX_FLAGS_MASKALL;
+        pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
 
         list_for_each_entry(entry, &dev->msi_list, list) {
                 write_msi_msg(entry->irq, &entry->msg);
                 msix_mask_irq(entry, entry->masked);
         }
 
-        BUG_ON(list_empty(&dev->msi_list));
-        entry = list_entry(dev->msi_list.next, struct msi_desc, list);
-        pos = entry->msi_attrib.pos;
-        pci_read_config_word(dev, pos + PCI_MSIX_FLAGS, &control);
         control &= ~PCI_MSIX_FLAGS_MASKALL;
-        control |= PCI_MSIX_FLAGS_ENABLE;
         pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
 }
 
@@ -365,9 +357,9 @@ static int msi_capability_init(struct pci_dev *dev, int nvec)
         u16 control;
         unsigned mask;
 
-        msi_set_enable(dev, 0); /* Ensure msi is disabled as I set it up */
-
         pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
+        msi_set_enable(dev, pos, 0);    /* Disable MSI during set up */
+
         pci_read_config_word(dev, msi_control_reg(pos), &control);
         /* MSI Entry Initialization */
         entry = alloc_msi_entry(dev);
@@ -381,7 +373,7 @@ static int msi_capability_init(struct pci_dev *dev, int nvec)
         entry->msi_attrib.default_irq = dev->irq;       /* Save IOAPIC IRQ */
         entry->msi_attrib.pos = pos;
 
-        entry->mask_pos = msi_mask_bits_reg(pos, entry->msi_attrib.is_64);
+        entry->mask_pos = msi_mask_reg(pos, entry->msi_attrib.is_64);
         /* All MSIs are unmasked by default, Mask them all */
         if (entry->msi_attrib.maskbit)
                 pci_read_config_dword(dev, entry->mask_pos, &entry->masked);
@@ -399,7 +391,7 @@ static int msi_capability_init(struct pci_dev *dev, int nvec)
 
         /* Set MSI enabled bits  */
         pci_intx_for_msi(dev, 0);
-        msi_set_enable(dev, 1);
+        msi_set_enable(dev, pos, 1);
         dev->msi_enabled = 1;
 
         dev->irq = entry->irq;
@@ -427,11 +419,14 @@ static int msix_capability_init(struct pci_dev *dev,
         u8 bir;
         void __iomem *base;
 
-        msix_set_enable(dev, 0);/* Ensure msix is disabled as I set it up */
-
         pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
+        pci_read_config_word(dev, pos + PCI_MSIX_FLAGS, &control);
+
+        /* Ensure MSI-X is disabled while it is set up */
+        control &= ~PCI_MSIX_FLAGS_ENABLE;
+        pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
+
         /* Request & Map MSI-X table region */
-        pci_read_config_word(dev, msi_control_reg(pos), &control);
         nr_entries = multi_msix_capable(control);
 
         pci_read_config_dword(dev, msix_table_offset_reg(pos), &table_offset);
@@ -442,7 +437,6 @@ static int msix_capability_init(struct pci_dev *dev,
         if (base == NULL)
                 return -ENOMEM;
 
-        /* MSI-X Table Initialization */
         for (i = 0; i < nvec; i++) {
                 entry = alloc_msi_entry(dev);
                 if (!entry)
@@ -455,7 +449,6 @@ static int msix_capability_init(struct pci_dev *dev,
                 entry->msi_attrib.default_irq = dev->irq;
                 entry->msi_attrib.pos = pos;
                 entry->mask_base = base;
-                msix_mask_irq(entry, 1);
 
                 list_add_tail(&entry->list, &dev->msi_list);
         }
@@ -480,22 +473,31 @@ static int msix_capability_init(struct pci_dev *dev,
                 return ret;
         }
 
+        /*
+         * Some devices require MSI-X to be enabled before we can touch the
+         * MSI-X registers.  We need to mask all the vectors to prevent
+         * interrupts coming in before they're fully set up.
+         */
+        control |= PCI_MSIX_FLAGS_MASKALL | PCI_MSIX_FLAGS_ENABLE;
+        pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
+
         i = 0;
         list_for_each_entry(entry, &dev->msi_list, list) {
                 entries[i].vector = entry->irq;
                 set_irq_msi(entry->irq, entry);
+                j = entries[i].entry;
+                entry->masked = readl(base + j * PCI_MSIX_ENTRY_SIZE +
+                                        PCI_MSIX_ENTRY_VECTOR_CTRL_OFFSET);
+                msix_mask_irq(entry, 1);
                 i++;
         }
-        /* Set MSI-X enabled bits */
+
+        /* Set MSI-X enabled bits and unmask the function */
         pci_intx_for_msi(dev, 0);
-        msix_set_enable(dev, 1);
         dev->msix_enabled = 1;
 
-        list_for_each_entry(entry, &dev->msi_list, list) {
-                int vector = entry->msi_attrib.entry_nr;
-                entry->masked = readl(base + vector * PCI_MSIX_ENTRY_SIZE +
-                                        PCI_MSIX_ENTRY_VECTOR_CTRL_OFFSET);
-        }
+        control &= ~PCI_MSIX_FLAGS_MASKALL;
+        pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
 
         return 0;
 }
@@ -596,17 +598,20 @@ void pci_msi_shutdown(struct pci_dev *dev)
         struct msi_desc *desc;
         u32 mask;
         u16 ctrl;
+        unsigned pos;
 
         if (!pci_msi_enable || !dev || !dev->msi_enabled)
                 return;
 
-        msi_set_enable(dev, 0);
+        BUG_ON(list_empty(&dev->msi_list));
+        desc = list_first_entry(&dev->msi_list, struct msi_desc, list);
+        pos = desc->msi_attrib.pos;
+
+        msi_set_enable(dev, pos, 0);
         pci_intx_for_msi(dev, 1);
         dev->msi_enabled = 0;
 
-        BUG_ON(list_empty(&dev->msi_list));
-        desc = list_first_entry(&dev->msi_list, struct msi_desc, list);
-        pci_read_config_word(dev, desc->msi_attrib.pos + PCI_MSI_FLAGS, &ctrl);
+        pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &ctrl);
         mask = msi_capable_mask(ctrl);
         msi_mask_irq(desc, mask, ~mask);
 
@@ -648,10 +653,7 @@ static int msi_free_irqs(struct pci_dev* dev)
 
         list_for_each_entry_safe(entry, tmp, &dev->msi_list, list) {
                 if (entry->msi_attrib.is_msix) {
-                        writel(1, entry->mask_base + entry->msi_attrib.entry_nr
-                                  * PCI_MSIX_ENTRY_SIZE
-                                  + PCI_MSIX_ENTRY_VECTOR_CTRL_OFFSET);
-
+                        msix_mask_irq(entry, 1);
                         if (list_is_last(&entry->list, &dev->msi_list))
                                 iounmap(entry->mask_base);
                 }
@@ -691,8 +693,8 @@ int pci_msix_table_size(struct pci_dev *dev)
  * indicates the successful configuration of MSI-X capability structure
  * with new allocated MSI-X irqs. A return of < 0 indicates a failure.
  * Or a return of > 0 indicates that driver request is exceeding the number
- * of irqs available. Driver should use the returned value to re-send
- * its request.
+ * of irqs or MSI-X vectors available. Driver should use the returned value to
+ * re-send its request.
  **/
 int pci_enable_msix(struct pci_dev* dev, struct msix_entry *entries, int nvec)
 {
@@ -708,7 +710,7 @@ int pci_enable_msix(struct pci_dev* dev, struct msix_entry *entries, int nvec)
 
         nr_entries = pci_msix_table_size(dev);
         if (nvec > nr_entries)
-                return -EINVAL;
+                return nr_entries;
 
         /* Check for any invalid entries */
         for (i = 0; i < nvec; i++) {
diff --git a/drivers/pci/msi.h b/drivers/pci/msi.h
index 71f4df2ef654..a0662842550b 100644
--- a/drivers/pci/msi.h
+++ b/drivers/pci/msi.h
@@ -16,21 +16,15 @@
 #define msi_lower_address_reg(base)     (base + PCI_MSI_ADDRESS_LO)
 #define msi_upper_address_reg(base)     (base + PCI_MSI_ADDRESS_HI)
 #define msi_data_reg(base, is64bit)     \
-        ( (is64bit == 1) ? base+PCI_MSI_DATA_64 : base+PCI_MSI_DATA_32 )
-#define msi_mask_bits_reg(base, is64bit) \
-        ( (is64bit == 1) ? base+PCI_MSI_MASK_BIT : base+PCI_MSI_MASK_BIT-4)
-#define msi_disable(control)            control &= ~PCI_MSI_FLAGS_ENABLE
+        (base + ((is64bit == 1) ? PCI_MSI_DATA_64 : PCI_MSI_DATA_32))
+#define msi_mask_reg(base, is64bit)     \
+        (base + ((is64bit == 1) ? PCI_MSI_MASK_64 : PCI_MSI_MASK_32))
 #define is_64bit_address(control)       (!!(control & PCI_MSI_FLAGS_64BIT))
 #define is_mask_bit_support(control)    (!!(control & PCI_MSI_FLAGS_MASKBIT))
 
 #define msix_table_offset_reg(base)     (base + 0x04)
 #define msix_pba_offset_reg(base)       (base + 0x08)
-#define msix_enable(control)            control |= PCI_MSIX_FLAGS_ENABLE
-#define msix_disable(control)           control &= ~PCI_MSIX_FLAGS_ENABLE
 #define msix_table_size(control)        ((control & PCI_MSIX_FLAGS_QSIZE)+1)
-#define multi_msix_capable              msix_table_size
-#define msix_unmask(address)            (address & ~PCI_MSIX_FLAGS_BITMASK)
-#define msix_mask(address)              (address | PCI_MSIX_FLAGS_BITMASK)
-#define msix_is_pending(address)        (address & PCI_MSIX_FLAGS_PENDMASK)
+#define multi_msix_capable(control)     msix_table_size((control))
 
 #endif /* MSI_H */
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 07bbb9b3b93f..6c93af5ced18 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -485,6 +485,8 @@ static int pci_raw_set_power_state(struct pci_dev *dev, pci_power_t state)
                 pmcsr &= ~PCI_PM_CTRL_STATE_MASK;
                 pmcsr |= state;
                 break;
+        case PCI_D3hot:
+        case PCI_D3cold:
         case PCI_UNKNOWN: /* Boot-up */
                 if ((pmcsr & PCI_PM_CTRL_STATE_MASK) == PCI_D3hot
                  && !(pmcsr & PCI_PM_CTRL_NO_SOFT_RESET))
@@ -1208,7 +1210,7 @@ void pci_pme_active(struct pci_dev *dev, bool enable)
  * Error code depending on the platform is returned if both the platform and
  * the native mechanism fail to enable the generation of wake-up events
  */
-int pci_enable_wake(struct pci_dev *dev, pci_power_t state, int enable)
+int pci_enable_wake(struct pci_dev *dev, pci_power_t state, bool enable)
 {
         int error = 0;
         bool pme_done = false;
@@ -1287,15 +1289,14 @@ pci_power_t pci_target_state(struct pci_dev *dev)
                 default:
                         target_state = state;
                 }
+        } else if (!dev->pm_cap) {
+                target_state = PCI_D0;
         } else if (device_may_wakeup(&dev->dev)) {
                 /*
                  * Find the deepest state from which the device can generate
                  * wake-up events, make it the target state and enable device
                  * to generate PME#.
                  */
-                if (!dev->pm_cap)
-                        return PCI_POWER_ERROR;
-
                 if (dev->pme_support) {
                         while (target_state
                               && !(dev->pme_support & (1 << target_state)))
@@ -1532,7 +1533,7 @@ pci_get_interrupt_pin(struct pci_dev *dev, struct pci_dev **bridge)
         if (!pin)
                 return -1;
 
-        while (dev->bus->parent) {
+        while (!pci_is_root_bus(dev->bus)) {
                 pin = pci_swizzle_interrupt_pin(dev, pin);
                 dev = dev->bus->self;
         }
@@ -1552,7 +1553,7 @@ u8 pci_common_swizzle(struct pci_dev *dev, u8 *pinp)
 {
         u8 pin = *pinp;
 
-        while (dev->bus->parent) {
+        while (!pci_is_root_bus(dev->bus)) {
                 pin = pci_swizzle_interrupt_pin(dev, pin);
                 dev = dev->bus->self;
         }
@@ -2058,111 +2059,177 @@ int pci_set_dma_seg_boundary(struct pci_dev *dev, unsigned long mask)
 EXPORT_SYMBOL(pci_set_dma_seg_boundary);
 #endif
 
-static int __pcie_flr(struct pci_dev *dev, int probe)
+static int pcie_flr(struct pci_dev *dev, int probe)
 {
-        u16 status;
+        int i;
+        int pos;
         u32 cap;
-        int exppos = pci_find_capability(dev, PCI_CAP_ID_EXP);
+        u16 status;
 
-        if (!exppos)
+        pos = pci_find_capability(dev, PCI_CAP_ID_EXP);
+        if (!pos)
                 return -ENOTTY;
-        pci_read_config_dword(dev, exppos + PCI_EXP_DEVCAP, &cap);
+
+        pci_read_config_dword(dev, pos + PCI_EXP_DEVCAP, &cap);
         if (!(cap & PCI_EXP_DEVCAP_FLR))
                 return -ENOTTY;
 
         if (probe)
                 return 0;
 
-        pci_block_user_cfg_access(dev);
-
         /* Wait for Transaction Pending bit clean */
-        pci_read_config_word(dev, exppos + PCI_EXP_DEVSTA, &status);
-        if (!(status & PCI_EXP_DEVSTA_TRPND))
-                goto transaction_done;
+        for (i = 0; i < 4; i++) {
+                if (i)
+                        msleep((1 << (i - 1)) * 100);
 
-        msleep(100);
-        pci_read_config_word(dev, exppos + PCI_EXP_DEVSTA, &status);
-        if (!(status & PCI_EXP_DEVSTA_TRPND))
-                goto transaction_done;
-
-        dev_info(&dev->dev, "Busy after 100ms while trying to reset; "
-                        "sleeping for 1 second\n");
-        ssleep(1);
-        pci_read_config_word(dev, exppos + PCI_EXP_DEVSTA, &status);
-        if (status & PCI_EXP_DEVSTA_TRPND)
-                dev_info(&dev->dev, "Still busy after 1s; "
-                                "proceeding with reset anyway\n");
-
-transaction_done:
-        pci_write_config_word(dev, exppos + PCI_EXP_DEVCTL,
+                pci_read_config_word(dev, pos + PCI_EXP_DEVSTA, &status);
+                if (!(status & PCI_EXP_DEVSTA_TRPND))
+                        goto clear;
+        }
+
+        dev_err(&dev->dev, "transaction is not cleared; "
+                        "proceeding with reset anyway\n");
+
+clear:
+        pci_write_config_word(dev, pos + PCI_EXP_DEVCTL,
                                 PCI_EXP_DEVCTL_BCR_FLR);
-        mdelay(100);
+        msleep(100);
 
-        pci_unblock_user_cfg_access(dev);
         return 0;
 }
 
-static int __pci_af_flr(struct pci_dev *dev, int probe)
+static int pci_af_flr(struct pci_dev *dev, int probe)
 {
-        int cappos = pci_find_capability(dev, PCI_CAP_ID_AF);
-        u8 status;
+        int i;
+        int pos;
         u8 cap;
+        u8 status;
 
-        if (!cappos)
+        pos = pci_find_capability(dev, PCI_CAP_ID_AF);
+        if (!pos)
                 return -ENOTTY;
-        pci_read_config_byte(dev, cappos + PCI_AF_CAP, &cap);
+
+        pci_read_config_byte(dev, pos + PCI_AF_CAP, &cap);
         if (!(cap & PCI_AF_CAP_TP) || !(cap & PCI_AF_CAP_FLR))
                 return -ENOTTY;
 
         if (probe)
                 return 0;
 
-        pci_block_user_cfg_access(dev);
-
         /* Wait for Transaction Pending bit clean */
-        pci_read_config_byte(dev, cappos + PCI_AF_STATUS, &status);
-        if (!(status & PCI_AF_STATUS_TP))
-                goto transaction_done;
+        for (i = 0; i < 4; i++) {
+                if (i)
+                        msleep((1 << (i - 1)) * 100);
+
+                pci_read_config_byte(dev, pos + PCI_AF_STATUS, &status);
+                if (!(status & PCI_AF_STATUS_TP))
+                        goto clear;
+        }
 
+        dev_err(&dev->dev, "transaction is not cleared; "
+                        "proceeding with reset anyway\n");
+
+clear:
+        pci_write_config_byte(dev, pos + PCI_AF_CTRL, PCI_AF_CTRL_FLR);
         msleep(100);
-        pci_read_config_byte(dev, cappos + PCI_AF_STATUS, &status);
-        if (!(status & PCI_AF_STATUS_TP))
-                goto transaction_done;
-
-        dev_info(&dev->dev, "Busy after 100ms while trying to"
-                        " reset; sleeping for 1 second\n");
-        ssleep(1);
-        pci_read_config_byte(dev, cappos + PCI_AF_STATUS, &status);
-        if (status & PCI_AF_STATUS_TP)
-                dev_info(&dev->dev, "Still busy after 1s; "
-                                "proceeding with reset anyway\n");
-
-transaction_done:
-        pci_write_config_byte(dev, cappos + PCI_AF_CTRL, PCI_AF_CTRL_FLR);
-        mdelay(100);
-
-        pci_unblock_user_cfg_access(dev);
+
         return 0;
 }
 
-static int __pci_reset_function(struct pci_dev *pdev, int probe)
+static int pci_pm_reset(struct pci_dev *dev, int probe)
 {
-        int res;
+        u16 csr;
+
+        if (!dev->pm_cap)
+                return -ENOTTY;
 
-        res = __pcie_flr(pdev, probe);
-        if (res != -ENOTTY)
-                return res;
+        pci_read_config_word(dev, dev->pm_cap + PCI_PM_CTRL, &csr);
+        if (csr & PCI_PM_CTRL_NO_SOFT_RESET)
+                return -ENOTTY;
 
-        res = __pci_af_flr(pdev, probe);
-        if (res != -ENOTTY)
-                return res;
+        if (probe)
+                return 0;
 
-        return res;
+        if (dev->current_state != PCI_D0)
+                return -EINVAL;
+
+        csr &= ~PCI_PM_CTRL_STATE_MASK;
+        csr |= PCI_D3hot;
+        pci_write_config_word(dev, dev->pm_cap + PCI_PM_CTRL, csr);
+        msleep(pci_pm_d3_delay);
+
+        csr &= ~PCI_PM_CTRL_STATE_MASK;
+        csr |= PCI_D0;
+        pci_write_config_word(dev, dev->pm_cap + PCI_PM_CTRL, csr);
+        msleep(pci_pm_d3_delay);
+
+        return 0;
+}
+
+static int pci_parent_bus_reset(struct pci_dev *dev, int probe)
+{
+        u16 ctrl;
+        struct pci_dev *pdev;
+
+        if (dev->subordinate)
+                return -ENOTTY;
+
+        list_for_each_entry(pdev, &dev->bus->devices, bus_list)
+                if (pdev != dev)
+                        return -ENOTTY;
+
+        if (probe)
+                return 0;
+
+        pci_read_config_word(dev->bus->self, PCI_BRIDGE_CONTROL, &ctrl);
+        ctrl |= PCI_BRIDGE_CTL_BUS_RESET;
+        pci_write_config_word(dev->bus->self, PCI_BRIDGE_CONTROL, ctrl);
+        msleep(100);
+
+        ctrl &= ~PCI_BRIDGE_CTL_BUS_RESET;
+        pci_write_config_word(dev->bus->self, PCI_BRIDGE_CONTROL, ctrl);
+        msleep(100);
+
+        return 0;
+}
+
+static int pci_dev_reset(struct pci_dev *dev, int probe)
+{
+        int rc;
+
+        might_sleep();
+
+        if (!probe) {
+                pci_block_user_cfg_access(dev);
+                /* block PM suspend, driver probe, etc. */
+                down(&dev->dev.sem);
+        }
+
+        rc = pcie_flr(dev, probe);
+        if (rc != -ENOTTY)
+                goto done;
+
+        rc = pci_af_flr(dev, probe);
+        if (rc != -ENOTTY)
+                goto done;
+
+        rc = pci_pm_reset(dev, probe);
+        if (rc != -ENOTTY)
+                goto done;
+
+        rc = pci_parent_bus_reset(dev, probe);
+done:
+        if (!probe) {
+                up(&dev->dev.sem);
+                pci_unblock_user_cfg_access(dev);
+        }
+
+        return rc;
 }
 
 /**
- * pci_execute_reset_function() - Reset a PCI device function
- * @dev: Device function to reset
+ * __pci_reset_function - reset a PCI device function
+ * @dev: PCI device to reset
  *
  * Some devices allow an individual function to be reset without affecting
  * other functions in the same device.  The PCI device must be responsive
@@ -2174,18 +2241,18 @@ static int __pci_reset_function(struct pci_dev *pdev, int probe)
  * device including MSI, bus mastering, BARs, decoding IO and memory spaces,
  * etc.
  *
- * Returns 0 if the device function was successfully reset or -ENOTTY if the
+ * Returns 0 if the device function was successfully reset or negative if the
  * device doesn't support resetting a single function.
  */
-int pci_execute_reset_function(struct pci_dev *dev)
+int __pci_reset_function(struct pci_dev *dev)
 {
-        return __pci_reset_function(dev, 0);
+        return pci_dev_reset(dev, 0);
 }
-EXPORT_SYMBOL_GPL(pci_execute_reset_function);
+EXPORT_SYMBOL_GPL(__pci_reset_function);
 
 /**
- * pci_reset_function() - quiesce and reset a PCI device function
- * @dev: Device function to reset
+ * pci_reset_function - quiesce and reset a PCI device function
+ * @dev: PCI device to reset
  *
  * Some devices allow an individual function to be reset without affecting
  * other functions in the same device.  The PCI device must be responsive
@@ -2193,32 +2260,33 @@ EXPORT_SYMBOL_GPL(pci_execute_reset_function);
  *
  * This function does not just reset the PCI portion of a device, but
  * clears all the state associated with the device.  This function differs
- * from pci_execute_reset_function in that it saves and restores device state
+ * from __pci_reset_function in that it saves and restores device state
  * over the reset.
  *
- * Returns 0 if the device function was successfully reset or -ENOTTY if the
+ * Returns 0 if the device function was successfully reset or negative if the
  * device doesn't support resetting a single function.
  */
 int pci_reset_function(struct pci_dev *dev)
 {
-        int r = __pci_reset_function(dev, 1);
+        int rc;
 
-        if (r < 0)
-                return r;
+        rc = pci_dev_reset(dev, 1);
+        if (rc)
+                return rc;
 
-        if (!dev->msi_enabled && !dev->msix_enabled && dev->irq != 0)
-                disable_irq(dev->irq);
         pci_save_state(dev);
 
+        /*
+         * both INTx and MSI are disabled after the Interrupt Disable bit
+         * is set and the Bus Master bit is cleared.
+         */
         pci_write_config_word(dev, PCI_COMMAND, PCI_COMMAND_INTX_DISABLE);
 
-        r = pci_execute_reset_function(dev);
+        rc = pci_dev_reset(dev, 0);
 
         pci_restore_state(dev);
-        if (!dev->msi_enabled && !dev->msix_enabled && dev->irq != 0)
-                enable_irq(dev->irq);
 
-        return r;
+        return rc;
 }
 EXPORT_SYMBOL_GPL(pci_reset_function);
 
@@ -2591,6 +2659,8 @@ static int __init pci_setup(char *str)
                         } else if (!strncmp(str, "resource_alignment=", 19)) {
                                 pci_set_resource_alignment_param(str + 19,
                                                         strlen(str + 19));
+                        } else if (!strncmp(str, "ecrc=", 5)) {
+                                pcie_ecrc_get_policy(str + 5);
                         } else {
                                 printk(KERN_ERR "PCI: Unknown option `%s'\n",
                                                 str);
diff --git a/drivers/pci/pci.h b/drivers/pci/pci.h
index d03f6b99f292..f73bcbedf37c 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -229,6 +229,15 @@ struct pci_sriov {
         u8 __iomem *mstate;     /* VF Migration State Array */
 };
 
+/* Address Translation Service */
+struct pci_ats {
+        int pos;        /* capability position */
+        int stu;        /* Smallest Translation Unit */
+        int qdep;       /* Invalidate Queue Depth */
+        int ref_cnt;    /* Physical Function reference count */
+        int is_enabled:1;       /* Enable bit is set */
+};
+
 #ifdef CONFIG_PCI_IOV
 extern int pci_iov_init(struct pci_dev *dev);
 extern void pci_iov_release(struct pci_dev *dev);
@@ -236,6 +245,20 @@ extern int pci_iov_resource_bar(struct pci_dev *dev, int resno,
                                 enum pci_bar_type *type);
 extern void pci_restore_iov_state(struct pci_dev *dev);
 extern int pci_iov_bus_range(struct pci_bus *bus);
+
+extern int pci_enable_ats(struct pci_dev *dev, int ps);
+extern void pci_disable_ats(struct pci_dev *dev);
+extern int pci_ats_queue_depth(struct pci_dev *dev);
+/**
+ * pci_ats_enabled - query the ATS status
+ * @dev: the PCI device
+ *
+ * Returns 1 if ATS capability is enabled, or 0 if not.
+ */
+static inline int pci_ats_enabled(struct pci_dev *dev)
+{
+        return dev->ats && dev->ats->is_enabled;
+}
 #else
 static inline int pci_iov_init(struct pci_dev *dev)
 {
@@ -257,6 +280,22 @@ static inline int pci_iov_bus_range(struct pci_bus *bus)
 {
         return 0;
 }
+
+static inline int pci_enable_ats(struct pci_dev *dev, int ps)
+{
+        return -ENODEV;
+}
+static inline void pci_disable_ats(struct pci_dev *dev)
+{
+}
+static inline int pci_ats_queue_depth(struct pci_dev *dev)
+{
+        return -ENODEV;
+}
+static inline int pci_ats_enabled(struct pci_dev *dev)
+{
+        return 0;
+}
 #endif /* CONFIG_PCI_IOV */
 
 #endif /* DRIVERS_PCI_H */
diff --git a/drivers/pci/pcie/aer/Kconfig b/drivers/pci/pcie/aer/Kconfig
index c3bde588aa13..50e94e02378a 100644
--- a/drivers/pci/pcie/aer/Kconfig
+++ b/drivers/pci/pcie/aer/Kconfig
@@ -10,3 +10,18 @@ config PCIEAER
           This enables PCI Express Root Port Advanced Error Reporting
           (AER) driver support. Error reporting messages sent to Root
           Port will be handled by PCI Express AER driver.
+
+
+#
+# PCI Express ECRC
+#
+config PCIE_ECRC
+        bool "PCI Express ECRC settings control"
+        depends on PCIEAER
+        help
+          Used to override firmware/bios settings for PCI Express ECRC
+          (transaction layer end-to-end CRC checking).
+
+          When in doubt, say N.
+
+source "drivers/pci/pcie/aer/Kconfig.debug"
diff --git a/drivers/pci/pcie/aer/Kconfig.debug b/drivers/pci/pcie/aer/Kconfig.debug
new file mode 100644
index 000000000000..b8c925c1f6aa
--- /dev/null
+++ b/drivers/pci/pcie/aer/Kconfig.debug
@@ -0,0 +1,18 @@
+#
+# PCI Express Root Port Device AER Debug Configuration
+#
+
+config PCIEAER_INJECT
+        tristate "PCIE AER error injector support"
+        depends on PCIEAER
+        default n
+        help
+          This enables PCI Express Root Port Advanced Error Reporting
+          (AER) software error injector.
+
+          Debuging PCIE AER code is quite difficult because it is hard
+          to trigger various real hardware errors. Software based
+          error injection can fake almost all kinds of errors with the
+          help of a user space helper tool aer-inject, which can be
+          gotten from:
+             http://www.kernel.org/pub/linux/utils/pci/aer-inject/
diff --git a/drivers/pci/pcie/aer/Makefile b/drivers/pci/pcie/aer/Makefile
index 8da3bd8455a8..2cba67510dc8 100644
--- a/drivers/pci/pcie/aer/Makefile
+++ b/drivers/pci/pcie/aer/Makefile
@@ -4,6 +4,9 @@
 
 obj-$(CONFIG_PCIEAER) += aerdriver.o
 
+obj-$(CONFIG_PCIE_ECRC) += ecrc.o
+
 aerdriver-objs := aerdrv_errprint.o aerdrv_core.o aerdrv.o
 aerdriver-$(CONFIG_ACPI) += aerdrv_acpi.o
 
+obj-$(CONFIG_PCIEAER_INJECT) += aer_inject.o
diff --git a/drivers/pci/pcie/aer/aer_inject.c b/drivers/pci/pcie/aer/aer_inject.c
new file mode 100644
index 000000000000..d92ae21a59d8
--- /dev/null
+++ b/drivers/pci/pcie/aer/aer_inject.c
@@ -0,0 +1,473 @@
+/*
+ * PCIE AER software error injection support.
+ *
+ * Debuging PCIE AER code is quite difficult because it is hard to
+ * trigger various real hardware errors. Software based error
+ * injection can fake almost all kinds of errors with the help of a
+ * user space helper tool aer-inject, which can be gotten from:
+ *   http://www.kernel.org/pub/linux/utils/pci/aer-inject/
+ *
+ * Copyright 2009 Intel Corporation.
+ *     Huang Ying <ying.huang@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; version 2
+ * of the License.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/miscdevice.h>
+#include <linux/pci.h>
+#include <linux/fs.h>
+#include <asm/uaccess.h>
+#include "aerdrv.h"
+
+struct aer_error_inj
+{
+        u8 bus;
+        u8 dev;
+        u8 fn;
+        u32 uncor_status;
+        u32 cor_status;
+        u32 header_log0;
+        u32 header_log1;
+        u32 header_log2;
+        u32 header_log3;
+};
+
+struct aer_error
+{
+        struct list_head list;
+        unsigned int bus;
+        unsigned int devfn;
+        int pos_cap_err;
+
+        u32 uncor_status;
+        u32 cor_status;
+        u32 header_log0;
+        u32 header_log1;
+        u32 header_log2;
+        u32 header_log3;
+        u32 root_status;
+        u32 source_id;
+};
+
+struct pci_bus_ops
+{
+        struct list_head list;
+        struct pci_bus *bus;
+        struct pci_ops *ops;
+};
+
+static LIST_HEAD(einjected);
+
+static LIST_HEAD(pci_bus_ops_list);
+
+/* Protect einjected and pci_bus_ops_list */
+static DEFINE_SPINLOCK(inject_lock);
+
+static void aer_error_init(struct aer_error *err, unsigned int bus,
+                           unsigned int devfn, int pos_cap_err)
+{
+        INIT_LIST_HEAD(&err->list);
+        err->bus = bus;
+        err->devfn = devfn;
+        err->pos_cap_err = pos_cap_err;
+}
+
+/* inject_lock must be held before calling */
+static struct aer_error *__find_aer_error(unsigned int bus, unsigned int devfn)
+{
+        struct aer_error *err;
+
+        list_for_each_entry(err, &einjected, list) {
+                if (bus == err->bus && devfn == err->devfn)
+                        return err;
+        }
+        return NULL;
+}
+
+/* inject_lock must be held before calling */
+static struct aer_error *__find_aer_error_by_dev(struct pci_dev *dev)
+{
+        return __find_aer_error(dev->bus->number, dev->devfn);
+}
+
+/* inject_lock must be held before calling */
+static struct pci_ops *__find_pci_bus_ops(struct pci_bus *bus)
+{
+        struct pci_bus_ops *bus_ops;
+
+        list_for_each_entry(bus_ops, &pci_bus_ops_list, list) {
+                if (bus_ops->bus == bus)
+                        return bus_ops->ops;
+        }
+        return NULL;
+}
+
+static struct pci_bus_ops *pci_bus_ops_pop(void)
+{
+        unsigned long flags;
+        struct pci_bus_ops *bus_ops = NULL;
+
+        spin_lock_irqsave(&inject_lock, flags);
+        if (list_empty(&pci_bus_ops_list))
+                bus_ops = NULL;
+        else {
+                struct list_head *lh = pci_bus_ops_list.next;
+                list_del(lh);
+                bus_ops = list_entry(lh, struct pci_bus_ops, list);
+        }
+        spin_unlock_irqrestore(&inject_lock, flags);
+        return bus_ops;
+}
+
+static u32 *find_pci_config_dword(struct aer_error *err, int where,
+                                  int *prw1cs)
+{
+        int rw1cs = 0;
+        u32 *target = NULL;
+
+        if (err->pos_cap_err == -1)
+                return NULL;
+
+        switch (where - err->pos_cap_err) {
+        case PCI_ERR_UNCOR_STATUS:
+                target = &err->uncor_status;
+                rw1cs = 1;
+                break;
+        case PCI_ERR_COR_STATUS:
+                target = &err->cor_status;
+                rw1cs = 1;
+                break;
+        case PCI_ERR_HEADER_LOG:
+                target = &err->header_log0;
+                break;
+        case PCI_ERR_HEADER_LOG+4:
+                target = &err->header_log1;
+                break;
+        case PCI_ERR_HEADER_LOG+8:
+                target = &err->header_log2;
+                break;
+        case PCI_ERR_HEADER_LOG+12:
+                target = &err->header_log3;
+                break;
+        case PCI_ERR_ROOT_STATUS:
+                target = &err->root_status;
+                rw1cs = 1;
+                break;
+        case PCI_ERR_ROOT_COR_SRC:
+                target = &err->source_id;
+                break;
+        }
+        if (prw1cs)
+                *prw1cs = rw1cs;
+        return target;
+}
+
+static int pci_read_aer(struct pci_bus *bus, unsigned int devfn, int where,
+                        int size, u32 *val)
+{
+        u32 *sim;
+        struct aer_error *err;
+        unsigned long flags;
+        struct pci_ops *ops;
+
+        spin_lock_irqsave(&inject_lock, flags);
+        if (size != sizeof(u32))
+                goto out;
+        err = __find_aer_error(bus->number, devfn);
+        if (!err)
+                goto out;
+
+        sim = find_pci_config_dword(err, where, NULL);
+        if (sim) {
+                *val = *sim;
+                spin_unlock_irqrestore(&inject_lock, flags);
+                return 0;
+        }
+out:
+        ops = __find_pci_bus_ops(bus);
+        spin_unlock_irqrestore(&inject_lock, flags);
+        return ops->read(bus, devfn, where, size, val);
+}
+
+int pci_write_aer(struct pci_bus *bus, unsigned int devfn, int where, int size,
+                  u32 val)
+{
+        u32 *sim;
+        struct aer_error *err;
+        unsigned long flags;
+        int rw1cs;
+        struct pci_ops *ops;
+
+        spin_lock_irqsave(&inject_lock, flags);
+        if (size != sizeof(u32))
+                goto out;
+        err = __find_aer_error(bus->number, devfn);
+        if (!err)
+                goto out;
+
+        sim = find_pci_config_dword(err, where, &rw1cs);
+        if (sim) {
+                if (rw1cs)
+                        *sim ^= val;
+                else
+                        *sim = val;
+                spin_unlock_irqrestore(&inject_lock, flags);
+                return 0;
+        }
+out:
+        ops = __find_pci_bus_ops(bus);
+        spin_unlock_irqrestore(&inject_lock, flags);
+        return ops->write(bus, devfn, where, size, val);
+}
+
+static struct pci_ops pci_ops_aer = {
+        .read = pci_read_aer,
+        .write = pci_write_aer,
+};
+
+static void pci_bus_ops_init(struct pci_bus_ops *bus_ops,
+                             struct pci_bus *bus,
+                             struct pci_ops *ops)
+{
+        INIT_LIST_HEAD(&bus_ops->list);
+        bus_ops->bus = bus;
+        bus_ops->ops = ops;
+}
+
+static int pci_bus_set_aer_ops(struct pci_bus *bus)
+{
+        struct pci_ops *ops;
+        struct pci_bus_ops *bus_ops;
+        unsigned long flags;
+
+        bus_ops = kmalloc(sizeof(*bus_ops), GFP_KERNEL);
+        if (!bus_ops)
+                return -ENOMEM;
+        ops = pci_bus_set_ops(bus, &pci_ops_aer);
+        spin_lock_irqsave(&inject_lock, flags);
+        if (ops == &pci_ops_aer)
+                goto out;
+        pci_bus_ops_init(bus_ops, bus, ops);
+        list_add(&bus_ops->list, &pci_bus_ops_list);
+        bus_ops = NULL;
+out:
+        spin_unlock_irqrestore(&inject_lock, flags);
+        if (bus_ops)
+                kfree(bus_ops);
+        return 0;
+}
+
+static struct pci_dev *pcie_find_root_port(struct pci_dev *dev)
+{
+        while (1) {
+                if (!dev->is_pcie)
+                        break;
+                if (dev->pcie_type == PCI_EXP_TYPE_ROOT_PORT)
+                        return dev;
+                if (!dev->bus->self)
+                        break;
+                dev = dev->bus->self;
+        }
+        return NULL;
+}
+
+static int find_aer_device_iter(struct device *device, void *data)
+{
+        struct pcie_device **result = data;
+        struct pcie_device *pcie_dev;
+
+        if (device->bus == &pcie_port_bus_type) {
+                pcie_dev = to_pcie_device(device);
+                if (pcie_dev->service & PCIE_PORT_SERVICE_AER) {
+                        *result = pcie_dev;
+                        return 1;
+                }
+        }
+        return 0;
+}
+
+static int find_aer_device(struct pci_dev *dev, struct pcie_device **result)
+{
+        return device_for_each_child(&dev->dev, result, find_aer_device_iter);
+}
+
+static int aer_inject(struct aer_error_inj *einj)
+{
+        struct aer_error *err, *rperr;
+        struct aer_error *err_alloc = NULL, *rperr_alloc = NULL;
+        struct pci_dev *dev, *rpdev;
+        struct pcie_device *edev;
+        unsigned long flags;
+        unsigned int devfn = PCI_DEVFN(einj->dev, einj->fn);
+        int pos_cap_err, rp_pos_cap_err;
+        u32 sever;
+        int ret = 0;
+
+        dev = pci_get_bus_and_slot(einj->bus, devfn);
+        if (!dev)
+                return -EINVAL;
+        rpdev = pcie_find_root_port(dev);
+        if (!rpdev) {
+                ret = -EINVAL;
+                goto out_put;
+        }
+
+        pos_cap_err = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
+        if (!pos_cap_err) {
+                ret = -EIO;
+                goto out_put;
+        }
+        pci_read_config_dword(dev, pos_cap_err + PCI_ERR_UNCOR_SEVER, &sever);
+
+        rp_pos_cap_err = pci_find_ext_capability(rpdev, PCI_EXT_CAP_ID_ERR);
+        if (!rp_pos_cap_err) {
+                ret = -EIO;
+                goto out_put;
+        }
+
+        err_alloc =  kzalloc(sizeof(struct aer_error), GFP_KERNEL);
+        if (!err_alloc) {
+                ret = -ENOMEM;
+                goto out_put;
+        }
+        rperr_alloc =  kzalloc(sizeof(struct aer_error), GFP_KERNEL);
+        if (!rperr_alloc) {
+                ret = -ENOMEM;
+                goto out_put;
+        }
+
+        spin_lock_irqsave(&inject_lock, flags);
+
+        err = __find_aer_error_by_dev(dev);
+        if (!err) {
+                err = err_alloc;
+                err_alloc = NULL;
+                aer_error_init(err, einj->bus, devfn, pos_cap_err);
+                list_add(&err->list, &einjected);
+        }
+        err->uncor_status |= einj->uncor_status;
+        err->cor_status |= einj->cor_status;
+        err->header_log0 = einj->header_log0;
+        err->header_log1 = einj->header_log1;
+        err->header_log2 = einj->header_log2;
+        err->header_log3 = einj->header_log3;
+
+        rperr = __find_aer_error_by_dev(rpdev);
+        if (!rperr) {
+                rperr = rperr_alloc;
+                rperr_alloc = NULL;
+                aer_error_init(rperr, rpdev->bus->number, rpdev->devfn,
+                               rp_pos_cap_err);
+                list_add(&rperr->list, &einjected);
+        }
+        if (einj->cor_status) {
+                if (rperr->root_status & PCI_ERR_ROOT_COR_RCV)
+                        rperr->root_status |= PCI_ERR_ROOT_MULTI_COR_RCV;
+                else
+                        rperr->root_status |= PCI_ERR_ROOT_COR_RCV;
+                rperr->source_id &= 0xffff0000;
+                rperr->source_id |= (einj->bus << 8) | devfn;
+        }
+        if (einj->uncor_status) {
+                if (rperr->root_status & PCI_ERR_ROOT_UNCOR_RCV)
+                        rperr->root_status |= PCI_ERR_ROOT_MULTI_UNCOR_RCV;
+                if (sever & einj->uncor_status) {
+                        rperr->root_status |= PCI_ERR_ROOT_FATAL_RCV;
+                        if (!(rperr->root_status & PCI_ERR_ROOT_UNCOR_RCV))
+                                rperr->root_status |= PCI_ERR_ROOT_FIRST_FATAL;
+                } else
+                        rperr->root_status |= PCI_ERR_ROOT_NONFATAL_RCV;
+                rperr->root_status |= PCI_ERR_ROOT_UNCOR_RCV;
+                rperr->source_id &= 0x0000ffff;
+                rperr->source_id |= ((einj->bus << 8) | devfn) << 16;
+        }
+        spin_unlock_irqrestore(&inject_lock, flags);
+
+        ret = pci_bus_set_aer_ops(dev->bus);
+        if (ret)
+                goto out_put;
+        ret = pci_bus_set_aer_ops(rpdev->bus);
+        if (ret)
+                goto out_put;
+
+        if (find_aer_device(rpdev, &edev))
+                aer_irq(-1, edev);
+        else
+                ret = -EINVAL;
+out_put:
+        if (err_alloc)
+                kfree(err_alloc);
+        if (rperr_alloc)
+                kfree(rperr_alloc);
+        pci_dev_put(dev);
+        return ret;
+}
+
+static ssize_t aer_inject_write(struct file *filp, const char __user *ubuf,
+                                size_t usize, loff_t *off)
+{
+        struct aer_error_inj einj;
+        int ret;
+
+        if (!capable(CAP_SYS_ADMIN))
+                return -EPERM;
+
+        if (usize != sizeof(struct aer_error_inj))
+                return -EINVAL;
+
+        if (copy_from_user(&einj, ubuf, usize))
+                return -EFAULT;
+
+        ret = aer_inject(&einj);
+        return ret ? ret : usize;
+}
+
+static const struct file_operations aer_inject_fops = {
+        .write = aer_inject_write,
+        .owner = THIS_MODULE,
+};
+
+static struct miscdevice aer_inject_device = {
+        .minor = MISC_DYNAMIC_MINOR,
+        .name = "aer_inject",
+        .fops = &aer_inject_fops,
+};
+
+static int __init aer_inject_init(void)
+{
+        return misc_register(&aer_inject_device);
+}
+
+static void __exit aer_inject_exit(void)
+{
+        struct aer_error *err, *err_next;
+        unsigned long flags;
+        struct pci_bus_ops *bus_ops;
+
+        misc_deregister(&aer_inject_device);
+
+        while ((bus_ops = pci_bus_ops_pop())) {
+                pci_bus_set_ops(bus_ops->bus, bus_ops->ops);
+                kfree(bus_ops);
+        }
+
+        spin_lock_irqsave(&inject_lock, flags);
+        list_for_each_entry_safe(err, err_next,
+                                 &pci_bus_ops_list, list) {
+                list_del(&err->list);
+                kfree(err);
+        }
+        spin_unlock_irqrestore(&inject_lock, flags);
+}
+
+module_init(aer_inject_init);
+module_exit(aer_inject_exit);
+
+MODULE_DESCRIPTION("PCIE AER software error injector");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pci/pcie/aer/aerdrv.c b/drivers/pci/pcie/aer/aerdrv.c
index 32ade5af927e..4770f13b3ca1 100644
--- a/drivers/pci/pcie/aer/aerdrv.c
+++ b/drivers/pci/pcie/aer/aerdrv.c
@@ -77,7 +77,7 @@ void pci_no_aer(void)
  *
  * Invoked when Root Port detects AER messages.
  **/
-static irqreturn_t aer_irq(int irq, void *context)
+irqreturn_t aer_irq(int irq, void *context)
 {
         unsigned int status, id;
         struct pcie_device *pdev = (struct pcie_device *)context;
@@ -126,6 +126,7 @@ static irqreturn_t aer_irq(int irq, void *context)
 
         return IRQ_HANDLED;
 }
+EXPORT_SYMBOL_GPL(aer_irq);
 
 /**
  * aer_alloc_rpc - allocate Root Port data structure
diff --git a/drivers/pci/pcie/aer/aerdrv.h b/drivers/pci/pcie/aer/aerdrv.h
index aa14482a4779..bbd7428ca2d0 100644
--- a/drivers/pci/pcie/aer/aerdrv.h
+++ b/drivers/pci/pcie/aer/aerdrv.h
@@ -11,6 +11,7 @@
 #include <linux/workqueue.h>
 #include <linux/pcieport_if.h>
 #include <linux/aer.h>
+#include <linux/interrupt.h>
 
 #define AER_NONFATAL                    0
 #define AER_FATAL                       1
@@ -56,7 +57,11 @@ struct header_log_regs {
         unsigned int dw3;
 };
 
+#define AER_MAX_MULTI_ERR_DEVICES       5       /* Not likely to have more */
 struct aer_err_info {
+        struct pci_dev *dev[AER_MAX_MULTI_ERR_DEVICES];
+        int error_dev_num;
+        u16 id;
         int severity;                   /* 0:NONFATAL | 1:FATAL | 2:COR */
         int flags;
         unsigned int status;            /* COR/UNCOR Error Status */
@@ -120,6 +125,7 @@ extern void aer_delete_rootport(struct aer_rpc *rpc);
 extern int aer_init(struct pcie_device *dev);
 extern void aer_isr(struct work_struct *work);
 extern void aer_print_error(struct pci_dev *dev, struct aer_err_info *info);
+extern irqreturn_t aer_irq(int irq, void *context);
 
 #ifdef CONFIG_ACPI
 extern int aer_osc_setup(struct pcie_device *pciedev);
diff --git a/drivers/pci/pcie/aer/aerdrv_core.c b/drivers/pci/pcie/aer/aerdrv_core.c
index 307452f30035..3d8872704a58 100644
--- a/drivers/pci/pcie/aer/aerdrv_core.c
+++ b/drivers/pci/pcie/aer/aerdrv_core.c
@@ -26,7 +26,9 @@
 #include "aerdrv.h"
 
 static int forceload;
+static int nosourceid;
 module_param(forceload, bool, 0);
+module_param(nosourceid, bool, 0);
 
 int pci_enable_pcie_error_reporting(struct pci_dev *dev)
 {
@@ -109,19 +111,23 @@ int pci_cleanup_aer_correct_error_status(struct pci_dev *dev)
 #endif  /*  0  */
 
 
-static void set_device_error_reporting(struct pci_dev *dev, void *data)
+static int set_device_error_reporting(struct pci_dev *dev, void *data)
 {
         bool enable = *((bool *)data);
 
-        if (dev->pcie_type != PCIE_RC_PORT &&
-            dev->pcie_type != PCIE_SW_UPSTREAM_PORT &&
-            dev->pcie_type != PCIE_SW_DOWNSTREAM_PORT)
-                return;
+        if (dev->pcie_type == PCIE_RC_PORT ||
+            dev->pcie_type == PCIE_SW_UPSTREAM_PORT ||
+            dev->pcie_type == PCIE_SW_DOWNSTREAM_PORT) {
+                if (enable)
+                        pci_enable_pcie_error_reporting(dev);
+                else
+                        pci_disable_pcie_error_reporting(dev);
+        }
 
         if (enable)
-                pci_enable_pcie_error_reporting(dev);
-        else
-                pci_disable_pcie_error_reporting(dev);
+                pcie_set_ecrc_checking(dev);
+
+        return 0;
 }
 
 /**
@@ -139,73 +145,148 @@ static void set_downstream_devices_error_reporting(struct pci_dev *dev,
         pci_walk_bus(dev->subordinate, set_device_error_reporting, &enable);
 }
 
-static int find_device_iter(struct device *device, void *data)
+static inline int compare_device_id(struct pci_dev *dev,
+                        struct aer_err_info *e_info)
 {
-        struct pci_dev *dev;
-        u16 id = *(unsigned long *)data;
-        u8 secondary, subordinate, d_bus = id >> 8;
+        if (e_info->id == ((dev->bus->number << 8) | dev->devfn)) {
+                /*
+                 * Device ID match
+                 */
+                return 1;
+        }
 
-        if (device->bus == &pci_bus_type) {
-                dev = to_pci_dev(device);
-                if (id == ((dev->bus->number << 8) | dev->devfn)) {
-                        /*
-                         * Device ID match
-                         */
-                        *(unsigned long*)data = (unsigned long)device;
-                        return 1;
-                }
+        return 0;
+}
+
+static int add_error_device(struct aer_err_info *e_info, struct pci_dev *dev)
+{
+        if (e_info->error_dev_num < AER_MAX_MULTI_ERR_DEVICES) {
+                e_info->dev[e_info->error_dev_num] = dev;
+                e_info->error_dev_num++;
+                return 1;
+        } else
+                return 0;
+}
+
+
+#define PCI_BUS(x)      (((x) >> 8) & 0xff)
+
+static int find_device_iter(struct pci_dev *dev, void *data)
+{
+        int pos;
+        u32 status;
+        u32 mask;
+        u16 reg16;
+        int result;
+        struct aer_err_info *e_info = (struct aer_err_info *)data;
+
+        /*
+         * When bus id is equal to 0, it might be a bad id
+         * reported by root port.
+         */
+        if (!nosourceid && (PCI_BUS(e_info->id) != 0)) {
+                result = compare_device_id(dev, e_info);
+                if (result)
+                        add_error_device(e_info, dev);
 
                 /*
-                 * If device is P2P, check if it is an upstream?
+                 * If there is no multiple error, we stop
+                 * or continue based on the id comparing.
                  */
-                if (dev->hdr_type & PCI_HEADER_TYPE_BRIDGE) {
-                        pci_read_config_byte(dev, PCI_SECONDARY_BUS,
-                                &secondary);
-                        pci_read_config_byte(dev, PCI_SUBORDINATE_BUS,
-                                &subordinate);
-                        if (d_bus >= secondary && d_bus <= subordinate) {
-                                *(unsigned long*)data = (unsigned long)device;
-                                return 1;
-                        }
+                if (!(e_info->flags & AER_MULTI_ERROR_VALID_FLAG))
+                        return result;
+
+                /*
+                 * If there are multiple errors and id does match,
+                 * We need continue to search other devices under
+                 * the root port. Return 0 means that.
+                 */
+                if (result)
+                        return 0;
+        }
+
+        /*
+         * When either
+         *      1) nosourceid==y;
+         *      2) bus id is equal to 0. Some ports might lose the bus
+         *              id of error source id;
+         *      3) There are multiple errors and prior id comparing fails;
+         * We check AER status registers to find the initial reporter.
+         */
+        if (atomic_read(&dev->enable_cnt) == 0)
+                return 0;
+        pos = pci_find_capability(dev, PCI_CAP_ID_EXP);
+        if (!pos)
+                return 0;
+        /* Check if AER is enabled */
+        pci_read_config_word(dev, pos+PCI_EXP_DEVCTL, &reg16);
+        if (!(reg16 & (
+                PCI_EXP_DEVCTL_CERE |
+                PCI_EXP_DEVCTL_NFERE |
+                PCI_EXP_DEVCTL_FERE |
+                PCI_EXP_DEVCTL_URRE)))
+                return 0;
+        pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
+        if (!pos)
+                return 0;
+
+        status = 0;
+        mask = 0;
+        if (e_info->severity == AER_CORRECTABLE) {
+                pci_read_config_dword(dev,
+                                pos + PCI_ERR_COR_STATUS,
+                                &status);
+                pci_read_config_dword(dev,
+                                pos + PCI_ERR_COR_MASK,
+                                &mask);
+                if (status & ERR_CORRECTABLE_ERROR_MASK & ~mask) {
+                        add_error_device(e_info, dev);
+                        goto added;
+                }
+        } else {
+                pci_read_config_dword(dev,
+                                pos + PCI_ERR_UNCOR_STATUS,
+                                &status);
+                pci_read_config_dword(dev,
+                                pos + PCI_ERR_UNCOR_MASK,
+                                &mask);
+                if (status & ERR_UNCORRECTABLE_ERROR_MASK & ~mask) {
+                        add_error_device(e_info, dev);
+                        goto added;
                 }
         }
 
         return 0;
+
+added:
+        if (e_info->flags & AER_MULTI_ERROR_VALID_FLAG)
+                return 0;
+        else
+                return 1;
 }
 
 /**
  * find_source_device - search through device hierarchy for source device
  * @parent: pointer to Root Port pci_dev data structure
- * @id: device ID of agent who sends an error message to this Root Port
+ * @err_info: including detailed error information such like id
  *
  * Invoked when error is detected at the Root Port.
  */
-static struct device* find_source_device(struct pci_dev *parent, u16 id)
+static void find_source_device(struct pci_dev *parent,
+                struct aer_err_info *e_info)
 {
         struct pci_dev *dev = parent;
-        struct device *device;
-        unsigned long device_addr;
-        int status;
+        int result;
 
         /* Is Root Port an agent that sends error message? */
-        if (id == ((dev->bus->number << 8) | dev->devfn))
-                return &dev->dev;
-
-        do {
-                device_addr = id;
-                if ((status = device_for_each_child(&dev->dev,
-                        &device_addr, find_device_iter))) {
-                        device = (struct device*)device_addr;
-                        dev = to_pci_dev(device);
-                        if (id == ((dev->bus->number << 8) | dev->devfn))
-                                return device;
-                }
-        }while (status);
+        result = find_device_iter(dev, e_info);
+        if (result)
+                return;
 
-        return NULL;
+        pci_walk_bus(parent->subordinate, find_device_iter, e_info);
 }
 
-static void report_error_detected(struct pci_dev *dev, void *data)
+static int report_error_detected(struct pci_dev *dev, void *data)
 {
         pci_ers_result_t vote;
         struct pci_error_handlers *err_handler;
@@ -230,16 +311,16 @@ static void report_error_detected(struct pci_dev *dev, void *data)
                                    dev->driver ?
                                    "no AER-aware driver" : "no driver");
                 }
-                return;
+                return 0;
         }
 
         err_handler = dev->driver->err_handler;
         vote = err_handler->error_detected(dev, result_data->state);
         result_data->result = merge_result(result_data->result, vote);
-        return;
+        return 0;
 }
 
-static void report_mmio_enabled(struct pci_dev *dev, void *data)
+static int report_mmio_enabled(struct pci_dev *dev, void *data)
 {
         pci_ers_result_t vote;
         struct pci_error_handlers *err_handler;
@@ -249,15 +330,15 @@ static void report_mmio_enabled(struct pci_dev *dev, void *data)
         if (!dev->driver ||
                 !dev->driver->err_handler ||
                 !dev->driver->err_handler->mmio_enabled)
-                return;
+                return 0;
 
         err_handler = dev->driver->err_handler;
         vote = err_handler->mmio_enabled(dev);
         result_data->result = merge_result(result_data->result, vote);
-        return;
+        return 0;
 }
 
-static void report_slot_reset(struct pci_dev *dev, void *data)
+static int report_slot_reset(struct pci_dev *dev, void *data)
 {
         pci_ers_result_t vote;
         struct pci_error_handlers *err_handler;
@@ -267,15 +348,15 @@ static void report_slot_reset(struct pci_dev *dev, void *data)
         if (!dev->driver ||
                 !dev->driver->err_handler ||
                 !dev->driver->err_handler->slot_reset)
-                return;
+                return 0;
 
         err_handler = dev->driver->err_handler;
         vote = err_handler->slot_reset(dev);
         result_data->result = merge_result(result_data->result, vote);
-        return;
+        return 0;
 }
 
-static void report_resume(struct pci_dev *dev, void *data)
+static int report_resume(struct pci_dev *dev, void *data)
 {
         struct pci_error_handlers *err_handler;
 
@@ -284,11 +365,11 @@ static void report_resume(struct pci_dev *dev, void *data)
         if (!dev->driver ||
                 !dev->driver->err_handler ||
                 !dev->driver->err_handler->resume)
-                return;
+                return 0;
 
         err_handler = dev->driver->err_handler;
         err_handler->resume(dev);
-        return;
+        return 0;
 }
 
 /**
@@ -305,7 +386,7 @@ static void report_resume(struct pci_dev *dev, void *data)
 static pci_ers_result_t broadcast_error_message(struct pci_dev *dev,
         enum pci_channel_state state,
         char *error_mesg,
-        void (*cb)(struct pci_dev *, void *))
+        int (*cb)(struct pci_dev *, void *))
 {
         struct aer_broadcast_data result_data;
 
@@ -497,12 +578,12 @@ static pci_ers_result_t do_recovery(struct pcie_device *aerdev,
  */
 static void handle_error_source(struct pcie_device * aerdev,
         struct pci_dev *dev,
-        struct aer_err_info info)
+        struct aer_err_info *info)
 {
         pci_ers_result_t status = 0;
         int pos;
 
-        if (info.severity == AER_CORRECTABLE) {
+        if (info->severity == AER_CORRECTABLE) {
                 /*
                  * Correctable error does not need software intevention.
                  * No need to go through error recovery process.
@@ -510,9 +591,9 @@ static void handle_error_source(struct pcie_device * aerdev,
                 pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
                 if (pos)
                         pci_write_config_dword(dev, pos + PCI_ERR_COR_STATUS,
-                                        info.status);
+                                        info->status);
         } else {
-                status = do_recovery(aerdev, dev, info.severity);
+                status = do_recovery(aerdev, dev, info->severity);
                 if (status == PCI_ERS_RESULT_RECOVERED) {
                         dev_printk(KERN_DEBUG, &dev->dev, "AER driver "
                                    "successfully recovered\n");
@@ -661,6 +742,28 @@ static int get_device_error_info(struct pci_dev *dev, struct aer_err_info *info)
         return AER_SUCCESS;
 }
 
+static inline void aer_process_err_devices(struct pcie_device *p_device,
+                        struct aer_err_info *e_info)
+{
+        int i;
+
+        if (!e_info->dev[0]) {
+                dev_printk(KERN_DEBUG, &p_device->port->dev,
+                                "can't find device of ID%04x\n",
+                                e_info->id);
+        }
+
+        for (i = 0; i < e_info->error_dev_num && e_info->dev[i]; i++) {
+                if (get_device_error_info(e_info->dev[i], e_info) ==
+                                AER_SUCCESS) {
+                        aer_print_error(e_info->dev[i], e_info);
+                        handle_error_source(p_device,
+                                        e_info->dev[i],
+                                        e_info);
+                }
+        }
+}
+
 /**
  * aer_isr_one_error - consume an error detected by root port
  * @p_device: pointer to error root port service device
@@ -669,10 +772,16 @@ static int get_device_error_info(struct pci_dev *dev, struct aer_err_info *info)
 static void aer_isr_one_error(struct pcie_device *p_device,
                 struct aer_err_source *e_src)
 {
-        struct device *s_device;
-        struct aer_err_info e_info = {0, 0, 0,};
+        struct aer_err_info *e_info;
         int i;
-        u16 id;
+
+        /* struct aer_err_info might be big, so we allocate it with slab */
+        e_info = kmalloc(sizeof(struct aer_err_info), GFP_KERNEL);
+        if (e_info == NULL) {
+                dev_printk(KERN_DEBUG, &p_device->port->dev,
+                        "Can't allocate mem when processing AER errors\n");
+                return;
+        }
 
         /*
          * There is a possibility that both correctable error and
@@ -684,31 +793,26 @@ static void aer_isr_one_error(struct pcie_device *p_device,
                 if (!(e_src->status & i))
                         continue;
 
+                memset(e_info, 0, sizeof(struct aer_err_info));
+
                 /* Init comprehensive error information */
                 if (i & PCI_ERR_ROOT_COR_RCV) {
-                        id = ERR_COR_ID(e_src->id);
-                        e_info.severity = AER_CORRECTABLE;
+                        e_info->id = ERR_COR_ID(e_src->id);
+                        e_info->severity = AER_CORRECTABLE;
                 } else {
-                        id = ERR_UNCOR_ID(e_src->id);
-                        e_info.severity = ((e_src->status >> 6) & 1);
+                        e_info->id = ERR_UNCOR_ID(e_src->id);
+                        e_info->severity = ((e_src->status >> 6) & 1);
                 }
                 if (e_src->status &
                         (PCI_ERR_ROOT_MULTI_COR_RCV |
                          PCI_ERR_ROOT_MULTI_UNCOR_RCV))
-                        e_info.flags |= AER_MULTI_ERROR_VALID_FLAG;
-                if (!(s_device = find_source_device(p_device->port, id))) {
-                        printk(KERN_DEBUG "%s->can't find device of ID%04x\n",
-                                __func__, id);
-                        continue;
-                }
-                if (get_device_error_info(to_pci_dev(s_device), &e_info) ==
-                                AER_SUCCESS) {
-                        aer_print_error(to_pci_dev(s_device), &e_info);
-                        handle_error_source(p_device,
-                                to_pci_dev(s_device),
-                                e_info);
-                }
+                        e_info->flags |= AER_MULTI_ERROR_VALID_FLAG;
+
+                find_source_device(p_device->port, e_info);
+                aer_process_err_devices(p_device, e_info);
         }
+
+        kfree(e_info);
 }
 
 /**
diff --git a/drivers/pci/pcie/aer/ecrc.c b/drivers/pci/pcie/aer/ecrc.c
new file mode 100644
index 000000000000..ece97df4df6d
--- /dev/null
+++ b/drivers/pci/pcie/aer/ecrc.c
@@ -0,0 +1,131 @@
+/*
+ *    Enables/disables PCIe ECRC checking.
+ *
+ *    (C) Copyright 2009 Hewlett-Packard Development Company, L.P.
+ *    Andrew Patterson <andrew.patterson@hp.com>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; version 2 of the License.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ *    General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program; if not, write to the Free Software
+ *    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
+ *    02111-1307, USA.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/pci.h>
+#include <linux/pci_regs.h>
+#include <linux/errno.h>
+#include "../../pci.h"
+
+#define ECRC_POLICY_DEFAULT 0           /* ECRC set by BIOS */
+#define ECRC_POLICY_OFF     1           /* ECRC off for performance */
+#define ECRC_POLICY_ON      2           /* ECRC on for data integrity */
+
+static int ecrc_policy = ECRC_POLICY_DEFAULT;
+
+static const char *ecrc_policy_str[] = {
+        [ECRC_POLICY_DEFAULT] = "bios",
+        [ECRC_POLICY_OFF] = "off",
+        [ECRC_POLICY_ON] = "on"
+};
+
+/**
+ * enable_ercr_checking - enable PCIe ECRC checking for a device
+ * @dev: the PCI device
+ *
+ * Returns 0 on success, or negative on failure.
+ */
+static int enable_ecrc_checking(struct pci_dev *dev)
+{
+        int pos;
+        u32 reg32;
+
+        if (!dev->is_pcie)
+                return -ENODEV;
+
+        pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
+        if (!pos)
+                return -ENODEV;
+
+        pci_read_config_dword(dev, pos + PCI_ERR_CAP, &reg32);
+        if (reg32 & PCI_ERR_CAP_ECRC_GENC)
+                reg32 |= PCI_ERR_CAP_ECRC_GENE;
+        if (reg32 & PCI_ERR_CAP_ECRC_CHKC)
+                reg32 |= PCI_ERR_CAP_ECRC_CHKE;
+        pci_write_config_dword(dev, pos + PCI_ERR_CAP, reg32);
+
+        return 0;
+}
+
+/**
+ * disable_ercr_checking - disables PCIe ECRC checking for a device
+ * @dev: the PCI device
+ *
+ * Returns 0 on success, or negative on failure.
+ */
+static int disable_ecrc_checking(struct pci_dev *dev)
+{
+        int pos;
+        u32 reg32;
+
+        if (!dev->is_pcie)
+                return -ENODEV;
+
+        pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
+        if (!pos)
+                return -ENODEV;
+
+        pci_read_config_dword(dev, pos + PCI_ERR_CAP, &reg32);
+        reg32 &= ~(PCI_ERR_CAP_ECRC_GENE | PCI_ERR_CAP_ECRC_CHKE);
+        pci_write_config_dword(dev, pos + PCI_ERR_CAP, reg32);
+
+        return 0;
+}
+
+/**
+ * pcie_set_ecrc_checking - set/unset PCIe ECRC checking for a device based on global policy
+ * @dev: the PCI device
+ */
+void pcie_set_ecrc_checking(struct pci_dev *dev)
+{
+        switch (ecrc_policy) {
+        case ECRC_POLICY_DEFAULT:
+                return;
+        case ECRC_POLICY_OFF:
+                disable_ecrc_checking(dev);
+                break;
+        case ECRC_POLICY_ON:
+                enable_ecrc_checking(dev);;
+                break;
+        default:
+                return;
+        }
+}
+
+/**
+ * pcie_ecrc_get_policy - parse kernel command-line ecrc option
+ */
+void pcie_ecrc_get_policy(char *str)
+{
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(ecrc_policy_str); i++)
+                if (!strncmp(str, ecrc_policy_str[i],
+                             strlen(ecrc_policy_str[i])))
+                        break;
+        if (i >= ARRAY_SIZE(ecrc_policy_str))
+                return;
+
+        ecrc_policy = i;
+}
diff --git a/drivers/pci/pcie/aspm.c b/drivers/pci/pcie/aspm.c
index b0367f168af4..3d27c97e0486 100644
--- a/drivers/pci/pcie/aspm.c
+++ b/drivers/pci/pcie/aspm.c
@@ -26,40 +26,36 @@
 #endif
 #define MODULE_PARAM_PREFIX "pcie_aspm."
 
-struct endpoint_state {
-        unsigned int l0s_acceptable_latency;
-        unsigned int l1_acceptable_latency;
+struct aspm_latency {
+        u32 l0s;                        /* L0s latency (nsec) */
+        u32 l1;                         /* L1 latency (nsec) */
 };
 
 struct pcie_link_state {
-        struct list_head sibiling;
-        struct pci_dev *pdev;
-        bool downstream_has_switch;
-
-        struct pcie_link_state *parent;
-        struct list_head children;
-        struct list_head link;
+        struct pci_dev *pdev;           /* Upstream component of the Link */
+        struct pcie_link_state *root;   /* pointer to the root port link */
+        struct pcie_link_state *parent; /* pointer to the parent Link state */
+        struct list_head sibling;       /* node in link_list */
+        struct list_head children;      /* list of child link states */
+        struct list_head link;          /* node in parent's children list */
 
         /* ASPM state */
-        unsigned int support_state;
-        unsigned int enabled_state;
-        unsigned int bios_aspm_state;
-        /* upstream component */
-        unsigned int l0s_upper_latency;
-        unsigned int l1_upper_latency;
-        /* downstream component */
-        unsigned int l0s_down_latency;
-        unsigned int l1_down_latency;
-        /* Clock PM state*/
-        unsigned int clk_pm_capable;
-        unsigned int clk_pm_enabled;
-        unsigned int bios_clk_state;
+        u32 aspm_support:2;             /* Supported ASPM state */
+        u32 aspm_enabled:2;             /* Enabled ASPM state */
+        u32 aspm_default:2;             /* Default ASPM state by BIOS */
+
+        /* Clock PM state */
+        u32 clkpm_capable:1;            /* Clock PM capable? */
+        u32 clkpm_enabled:1;            /* Current Clock PM state */
+        u32 clkpm_default:1;            /* Default Clock PM state by BIOS */
 
+        /* Latencies */
+        struct aspm_latency latency;    /* Exit latency */
         /*
-         * A pcie downstream port only has one slot under it, so at most there
-         * are 8 functions
+         * Endpoint acceptable latencies. A pcie downstream port only
+         * has one slot under it, so at most there are 8 functions.
          */
-        struct endpoint_state endpoints[8];
+        struct aspm_latency acceptable[8];
 };
 
 static int aspm_disabled, aspm_force;
@@ -78,27 +74,23 @@ static const char *policy_str[] = {
 
 #define LINK_RETRAIN_TIMEOUT HZ
 
-static int policy_to_aspm_state(struct pci_dev *pdev)
+static int policy_to_aspm_state(struct pcie_link_state *link)
 {
-        struct pcie_link_state *link_state = pdev->link_state;
-
         switch (aspm_policy) {
         case POLICY_PERFORMANCE:
                 /* Disable ASPM and Clock PM */
                 return 0;
         case POLICY_POWERSAVE:
                 /* Enable ASPM L0s/L1 */
-                return PCIE_LINK_STATE_L0S|PCIE_LINK_STATE_L1;
+                return PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1;
         case POLICY_DEFAULT:
-                return link_state->bios_aspm_state;
+                return link->aspm_default;
         }
         return 0;
 }
 
-static int policy_to_clkpm_state(struct pci_dev *pdev)
+static int policy_to_clkpm_state(struct pcie_link_state *link)
 {
-        struct pcie_link_state *link_state = pdev->link_state;
-
         switch (aspm_policy) {
         case POLICY_PERFORMANCE:
                 /* Disable ASPM and Clock PM */
@@ -107,73 +99,78 @@ static int policy_to_clkpm_state(struct pci_dev *pdev)
                 /* Disable Clock PM */
                 return 1;
         case POLICY_DEFAULT:
-                return link_state->bios_clk_state;
+                return link->clkpm_default;
         }
         return 0;
 }
 
-static void pcie_set_clock_pm(struct pci_dev *pdev, int enable)
+static void pcie_set_clkpm_nocheck(struct pcie_link_state *link, int enable)
 {
-        struct pci_dev *child_dev;
         int pos;
         u16 reg16;
-        struct pcie_link_state *link_state = pdev->link_state;
+        struct pci_dev *child;
+        struct pci_bus *linkbus = link->pdev->subordinate;
 
-        list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list) {
-                pos = pci_find_capability(child_dev, PCI_CAP_ID_EXP);
+        list_for_each_entry(child, &linkbus->devices, bus_list) {
+                pos = pci_find_capability(child, PCI_CAP_ID_EXP);
                 if (!pos)
                         return;
-                pci_read_config_word(child_dev, pos + PCI_EXP_LNKCTL, &reg16);
+                pci_read_config_word(child, pos + PCI_EXP_LNKCTL, &reg16);
                 if (enable)
                         reg16 |= PCI_EXP_LNKCTL_CLKREQ_EN;
                 else
                         reg16 &= ~PCI_EXP_LNKCTL_CLKREQ_EN;
-                pci_write_config_word(child_dev, pos + PCI_EXP_LNKCTL, reg16);
+                pci_write_config_word(child, pos + PCI_EXP_LNKCTL, reg16);
         }
-        link_state->clk_pm_enabled = !!enable;
+        link->clkpm_enabled = !!enable;
 }
 
-static void pcie_check_clock_pm(struct pci_dev *pdev, int blacklist)
+static void pcie_set_clkpm(struct pcie_link_state *link, int enable)
 {
-        int pos;
+        /* Don't enable Clock PM if the link is not Clock PM capable */
+        if (!link->clkpm_capable && enable)
+                return;
+        /* Need nothing if the specified equals to current state */
+        if (link->clkpm_enabled == enable)
+                return;
+        pcie_set_clkpm_nocheck(link, enable);
+}
+
+static void pcie_clkpm_cap_init(struct pcie_link_state *link, int blacklist)
+{
+        int pos, capable = 1, enabled = 1;
         u32 reg32;
         u16 reg16;
-        int capable = 1, enabled = 1;
-        struct pci_dev *child_dev;
-        struct pcie_link_state *link_state = pdev->link_state;
+        struct pci_dev *child;
+        struct pci_bus *linkbus = link->pdev->subordinate;
 
         /* All functions should have the same cap and state, take the worst */
-        list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list) {
-                pos = pci_find_capability(child_dev, PCI_CAP_ID_EXP);
+        list_for_each_entry(child, &linkbus->devices, bus_list) {
+                pos = pci_find_capability(child, PCI_CAP_ID_EXP);
                 if (!pos)
                         return;
-                pci_read_config_dword(child_dev, pos + PCI_EXP_LNKCAP, &reg32);
+                pci_read_config_dword(child, pos + PCI_EXP_LNKCAP, &reg32);
                 if (!(reg32 & PCI_EXP_LNKCAP_CLKPM)) {
                         capable = 0;
                         enabled = 0;
                         break;
                 }
-                pci_read_config_word(child_dev, pos + PCI_EXP_LNKCTL, &reg16);
+                pci_read_config_word(child, pos + PCI_EXP_LNKCTL, &reg16);
                 if (!(reg16 & PCI_EXP_LNKCTL_CLKREQ_EN))
                         enabled = 0;
         }
-        link_state->clk_pm_enabled = enabled;
-        link_state->bios_clk_state = enabled;
-        if (!blacklist) {
-                link_state->clk_pm_capable = capable;
-                pcie_set_clock_pm(pdev, policy_to_clkpm_state(pdev));
-        } else {
-                link_state->clk_pm_capable = 0;
-                pcie_set_clock_pm(pdev, 0);
-        }
+        link->clkpm_enabled = enabled;
+        link->clkpm_default = enabled;
+        link->clkpm_capable = (blacklist) ? 0 : capable;
 }
 
-static bool pcie_aspm_downstream_has_switch(struct pci_dev *pdev)
+static bool pcie_aspm_downstream_has_switch(struct pcie_link_state *link)
 {
-        struct pci_dev *child_dev;
+        struct pci_dev *child;
+        struct pci_bus *linkbus = link->pdev->subordinate;
 
-        list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list) {
-                if (child_dev->pcie_type == PCI_EXP_TYPE_UPSTREAM)
+        list_for_each_entry(child, &linkbus->devices, bus_list) {
+                if (child->pcie_type == PCI_EXP_TYPE_UPSTREAM)
                         return true;
         }
         return false;
@@ -184,289 +181,263 @@ static bool pcie_aspm_downstream_has_switch(struct pci_dev *pdev)
  *   could use common clock. If they are, configure them to use the
  *   common clock. That will reduce the ASPM state exit latency.
  */
-static void pcie_aspm_configure_common_clock(struct pci_dev *pdev)
+static void pcie_aspm_configure_common_clock(struct pcie_link_state *link)
 {
-        int pos, child_pos, i = 0;
-        u16 reg16 = 0;
-        struct pci_dev *child_dev;
-        int same_clock = 1;
+        int ppos, cpos, same_clock = 1;
+        u16 reg16, parent_reg, child_reg[8];
         unsigned long start_jiffies;
-        u16 child_regs[8], parent_reg;
+        struct pci_dev *child, *parent = link->pdev;
+        struct pci_bus *linkbus = parent->subordinate;
         /*
-         * all functions of a slot should have the same Slot Clock
+         * All functions of a slot should have the same Slot Clock
          * Configuration, so just check one function
-         * */
-        child_dev = list_entry(pdev->subordinate->devices.next, struct pci_dev,
-                bus_list);
-        BUG_ON(!child_dev->is_pcie);
+         */
+        child = list_entry(linkbus->devices.next, struct pci_dev, bus_list);
+        BUG_ON(!child->is_pcie);
 
         /* Check downstream component if bit Slot Clock Configuration is 1 */
-        child_pos = pci_find_capability(child_dev, PCI_CAP_ID_EXP);
-        pci_read_config_word(child_dev, child_pos + PCI_EXP_LNKSTA, &reg16);
+        cpos = pci_find_capability(child, PCI_CAP_ID_EXP);
+        pci_read_config_word(child, cpos + PCI_EXP_LNKSTA, &reg16);
         if (!(reg16 & PCI_EXP_LNKSTA_SLC))
                 same_clock = 0;
 
         /* Check upstream component if bit Slot Clock Configuration is 1 */
-        pos = pci_find_capability(pdev, PCI_CAP_ID_EXP);
-        pci_read_config_word(pdev, pos + PCI_EXP_LNKSTA, &reg16);
+        ppos = pci_find_capability(parent, PCI_CAP_ID_EXP);
+        pci_read_config_word(parent, ppos + PCI_EXP_LNKSTA, &reg16);
         if (!(reg16 & PCI_EXP_LNKSTA_SLC))
                 same_clock = 0;
 
         /* Configure downstream component, all functions */
-        list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list) {
-                child_pos = pci_find_capability(child_dev, PCI_CAP_ID_EXP);
-                pci_read_config_word(child_dev, child_pos + PCI_EXP_LNKCTL,
-                        &reg16);
-                child_regs[i] = reg16;
+        list_for_each_entry(child, &linkbus->devices, bus_list) {
+                cpos = pci_find_capability(child, PCI_CAP_ID_EXP);
+                pci_read_config_word(child, cpos + PCI_EXP_LNKCTL, &reg16);
+                child_reg[PCI_FUNC(child->devfn)] = reg16;
                 if (same_clock)
                         reg16 |= PCI_EXP_LNKCTL_CCC;
                 else
                         reg16 &= ~PCI_EXP_LNKCTL_CCC;
-                pci_write_config_word(child_dev, child_pos + PCI_EXP_LNKCTL,
-                        reg16);
-                i++;
+                pci_write_config_word(child, cpos + PCI_EXP_LNKCTL, reg16);
         }
 
         /* Configure upstream component */
-        pci_read_config_word(pdev, pos + PCI_EXP_LNKCTL, &reg16);
+        pci_read_config_word(parent, ppos + PCI_EXP_LNKCTL, &reg16);
         parent_reg = reg16;
         if (same_clock)
                 reg16 |= PCI_EXP_LNKCTL_CCC;
         else
                 reg16 &= ~PCI_EXP_LNKCTL_CCC;
-        pci_write_config_word(pdev, pos + PCI_EXP_LNKCTL, reg16);
+        pci_write_config_word(parent, ppos + PCI_EXP_LNKCTL, reg16);
 
-        /* retrain link */
+        /* Retrain link */
         reg16 |= PCI_EXP_LNKCTL_RL;
-        pci_write_config_word(pdev, pos + PCI_EXP_LNKCTL, reg16);
+        pci_write_config_word(parent, ppos + PCI_EXP_LNKCTL, reg16);
 
-        /* Wait for link training end */
-        /* break out after waiting for timeout */
+        /* Wait for link training end. Break out after waiting for timeout */
         start_jiffies = jiffies;
         for (;;) {
-                pci_read_config_word(pdev, pos + PCI_EXP_LNKSTA, &reg16);
+                pci_read_config_word(parent, ppos + PCI_EXP_LNKSTA, &reg16);
                 if (!(reg16 & PCI_EXP_LNKSTA_LT))
                         break;
                 if (time_after(jiffies, start_jiffies + LINK_RETRAIN_TIMEOUT))
                         break;
                 msleep(1);
         }
-        /* training failed -> recover */
-        if (reg16 & PCI_EXP_LNKSTA_LT) {
-                dev_printk (KERN_ERR, &pdev->dev, "ASPM: Could not configure"
-                            " common clock\n");
-                i = 0;
-                list_for_each_entry(child_dev, &pdev->subordinate->devices,
-                                    bus_list) {
-                        child_pos = pci_find_capability(child_dev,
-                                                        PCI_CAP_ID_EXP);
-                        pci_write_config_word(child_dev,
-                                              child_pos + PCI_EXP_LNKCTL,
-                                              child_regs[i]);
-                        i++;
-                }
-                pci_write_config_word(pdev, pos + PCI_EXP_LNKCTL, parent_reg);
+        if (!(reg16 & PCI_EXP_LNKSTA_LT))
+                return;
+
+        /* Training failed. Restore common clock configurations */
+        dev_printk(KERN_ERR, &parent->dev,
+                   "ASPM: Could not configure common clock\n");
+        list_for_each_entry(child, &linkbus->devices, bus_list) {
+                cpos = pci_find_capability(child, PCI_CAP_ID_EXP);
+                pci_write_config_word(child, cpos + PCI_EXP_LNKCTL,
+                                      child_reg[PCI_FUNC(child->devfn)]);
         }
+        pci_write_config_word(parent, ppos + PCI_EXP_LNKCTL, parent_reg);
 }
 
-/*
- * calc_L0S_latency: Convert L0s latency encoding to ns
- */
-static unsigned int calc_L0S_latency(unsigned int latency_encoding, int ac)
+/* Convert L0s latency encoding to ns */
+static u32 calc_l0s_latency(u32 encoding)
 {
-        unsigned int ns = 64;
+        if (encoding == 0x7)
+                return (5 * 1000);      /* > 4us */
+        return (64 << encoding);
+}
 
-        if (latency_encoding == 0x7) {
-                if (ac)
-                        ns = -1U;
-                else
-                        ns = 5*1000; /* > 4us */
-        } else
-                ns *= (1 << latency_encoding);
-        return ns;
+/* Convert L0s acceptable latency encoding to ns */
+static u32 calc_l0s_acceptable(u32 encoding)
+{
+        if (encoding == 0x7)
+                return -1U;
+        return (64 << encoding);
 }
 
-/*
- * calc_L1_latency: Convert L1 latency encoding to ns
- */
-static unsigned int calc_L1_latency(unsigned int latency_encoding, int ac)
+/* Convert L1 latency encoding to ns */
+static u32 calc_l1_latency(u32 encoding)
 {
-        unsigned int ns = 1000;
+        if (encoding == 0x7)
+                return (65 * 1000);     /* > 64us */
+        return (1000 << encoding);
+}
 
-        if (latency_encoding == 0x7) {
-                if (ac)
-                        ns = -1U;
-                else
-                        ns = 65*1000; /* > 64us */
-        } else
-                ns *= (1 << latency_encoding);
-        return ns;
+/* Convert L1 acceptable latency encoding to ns */
+static u32 calc_l1_acceptable(u32 encoding)
+{
+        if (encoding == 0x7)
+                return -1U;
+        return (1000 << encoding);
 }
 
 static void pcie_aspm_get_cap_device(struct pci_dev *pdev, u32 *state,
-        unsigned int *l0s, unsigned int *l1, unsigned int *enabled)
+                                     u32 *l0s, u32 *l1, u32 *enabled)
 {
         int pos;
         u16 reg16;
-        u32 reg32;
-        unsigned int latency;
+        u32 reg32, encoding;
 
+        *l0s = *l1 = *enabled = 0;
         pos = pci_find_capability(pdev, PCI_CAP_ID_EXP);
         pci_read_config_dword(pdev, pos + PCI_EXP_LNKCAP, &reg32);
         *state = (reg32 & PCI_EXP_LNKCAP_ASPMS) >> 10;
         if (*state != PCIE_LINK_STATE_L0S &&
-                *state != (PCIE_LINK_STATE_L1|PCIE_LINK_STATE_L0S))
+            *state != (PCIE_LINK_STATE_L1 | PCIE_LINK_STATE_L0S))
                 *state = 0;
         if (*state == 0)
                 return;
 
-        latency = (reg32 & PCI_EXP_LNKCAP_L0SEL) >> 12;
-        *l0s = calc_L0S_latency(latency, 0);
+        encoding = (reg32 & PCI_EXP_LNKCAP_L0SEL) >> 12;
+        *l0s = calc_l0s_latency(encoding);
         if (*state & PCIE_LINK_STATE_L1) {
-                latency = (reg32 & PCI_EXP_LNKCAP_L1EL) >> 15;
-                *l1 = calc_L1_latency(latency, 0);
+                encoding = (reg32 & PCI_EXP_LNKCAP_L1EL) >> 15;
+                *l1 = calc_l1_latency(encoding);
         }
         pci_read_config_word(pdev, pos + PCI_EXP_LNKCTL, &reg16);
-        *enabled = reg16 & (PCIE_LINK_STATE_L0S|PCIE_LINK_STATE_L1);
+        *enabled = reg16 & (PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1);
 }
 
-static void pcie_aspm_cap_init(struct pci_dev *pdev)
+static void pcie_aspm_cap_init(struct pcie_link_state *link, int blacklist)
 {
-        struct pci_dev *child_dev;
-        u32 state, tmp;
-        struct pcie_link_state *link_state = pdev->link_state;
+        u32 support, l0s, l1, enabled;
+        struct pci_dev *child, *parent = link->pdev;
+        struct pci_bus *linkbus = parent->subordinate;
+
+        if (blacklist) {
+                /* Set support state to 0, so we will disable ASPM later */
+                link->aspm_support = 0;
+                link->aspm_default = 0;
+                link->aspm_enabled = PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1;
+                return;
+        }
+
+        /* Configure common clock before checking latencies */
+        pcie_aspm_configure_common_clock(link);
 
         /* upstream component states */
-        pcie_aspm_get_cap_device(pdev, &link_state->support_state,
-                &link_state->l0s_upper_latency,
-                &link_state->l1_upper_latency,
-                &link_state->enabled_state);
+        pcie_aspm_get_cap_device(parent, &support, &l0s, &l1, &enabled);
+        link->aspm_support = support;
+        link->latency.l0s = l0s;
+        link->latency.l1 = l1;
+        link->aspm_enabled = enabled;
+
         /* downstream component states, all functions have the same setting */
-        child_dev = list_entry(pdev->subordinate->devices.next, struct pci_dev,
-                bus_list);
-        pcie_aspm_get_cap_device(child_dev, &state,
-                &link_state->l0s_down_latency,
-                &link_state->l1_down_latency,
-                &tmp);
-        link_state->support_state &= state;
-        if (!link_state->support_state)
+        child = list_entry(linkbus->devices.next, struct pci_dev, bus_list);
+        pcie_aspm_get_cap_device(child, &support, &l0s, &l1, &enabled);
+        link->aspm_support &= support;
+        link->latency.l0s = max_t(u32, link->latency.l0s, l0s);
+        link->latency.l1 = max_t(u32, link->latency.l1, l1);
+
+        if (!link->aspm_support)
                 return;
-        link_state->enabled_state &= link_state->support_state;
-        link_state->bios_aspm_state = link_state->enabled_state;
+
+        link->aspm_enabled &= link->aspm_support;
+        link->aspm_default = link->aspm_enabled;
 
         /* ENDPOINT states*/
-        list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list) {
+        list_for_each_entry(child, &linkbus->devices, bus_list) {
                 int pos;
-                u32 reg32;
-                unsigned int latency;
-                struct endpoint_state *ep_state =
-                        &link_state->endpoints[PCI_FUNC(child_dev->devfn)];
+                u32 reg32, encoding;
+                struct aspm_latency *acceptable =
+                        &link->acceptable[PCI_FUNC(child->devfn)];
 
-                if (child_dev->pcie_type != PCI_EXP_TYPE_ENDPOINT &&
-                        child_dev->pcie_type != PCI_EXP_TYPE_LEG_END)
+                if (child->pcie_type != PCI_EXP_TYPE_ENDPOINT &&
+                    child->pcie_type != PCI_EXP_TYPE_LEG_END)
                         continue;
 
-                pos = pci_find_capability(child_dev, PCI_CAP_ID_EXP);
-                pci_read_config_dword(child_dev, pos + PCI_EXP_DEVCAP, &reg32);
-                latency = (reg32 & PCI_EXP_DEVCAP_L0S) >> 6;
-                latency = calc_L0S_latency(latency, 1);
-                ep_state->l0s_acceptable_latency = latency;
-                if (link_state->support_state & PCIE_LINK_STATE_L1) {
-                        latency = (reg32 & PCI_EXP_DEVCAP_L1) >> 9;
-                        latency = calc_L1_latency(latency, 1);
-                        ep_state->l1_acceptable_latency = latency;
+                pos = pci_find_capability(child, PCI_CAP_ID_EXP);
+                pci_read_config_dword(child, pos + PCI_EXP_DEVCAP, &reg32);
+                encoding = (reg32 & PCI_EXP_DEVCAP_L0S) >> 6;
+                acceptable->l0s = calc_l0s_acceptable(encoding);
+                if (link->aspm_support & PCIE_LINK_STATE_L1) {
+                        encoding = (reg32 & PCI_EXP_DEVCAP_L1) >> 9;
+                        acceptable->l1 = calc_l1_acceptable(encoding);
                 }
         }
 }
 
-static unsigned int __pcie_aspm_check_state_one(struct pci_dev *pdev,
-        unsigned int state)
-{
-        struct pci_dev *parent_dev, *tmp_dev;
-        unsigned int latency, l1_latency = 0;
-        struct pcie_link_state *link_state;
-        struct endpoint_state *ep_state;
-
-        parent_dev = pdev->bus->self;
-        link_state = parent_dev->link_state;
-        state &= link_state->support_state;
-        if (state == 0)
-                return 0;
-        ep_state = &link_state->endpoints[PCI_FUNC(pdev->devfn)];
-
-        /*
-         * Check latency for endpoint device.
-         * TBD: The latency from the endpoint to root complex vary per
-         * switch's upstream link state above the device. Here we just do a
-         * simple check which assumes all links above the device can be in L1
-         * state, that is we just consider the worst case. If switch's upstream
-         * link can't be put into L0S/L1, then our check is too strictly.
-         */
-        tmp_dev = pdev;
-        while (state & (PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1)) {
-                parent_dev = tmp_dev->bus->self;
-                link_state = parent_dev->link_state;
-                if (state & PCIE_LINK_STATE_L0S) {
-                        latency = max_t(unsigned int,
-                                        link_state->l0s_upper_latency,
-                                        link_state->l0s_down_latency);
-                        if (latency > ep_state->l0s_acceptable_latency)
-                                state &= ~PCIE_LINK_STATE_L0S;
-                }
-                if (state & PCIE_LINK_STATE_L1) {
-                        latency = max_t(unsigned int,
-                                        link_state->l1_upper_latency,
-                                        link_state->l1_down_latency);
-                        if (latency + l1_latency >
-                                        ep_state->l1_acceptable_latency)
-                                state &= ~PCIE_LINK_STATE_L1;
-                }
-                if (!parent_dev->bus->self) /* parent_dev is a root port */
-                        break;
-                else {
-                        /*
-                         * parent_dev is the downstream port of a switch, make
-                         * tmp_dev the upstream port of the switch
-                         */
-                        tmp_dev = parent_dev->bus->self;
-                        /*
-                         * every switch on the path to root complex need 1 more
-                         * microsecond for L1. Spec doesn't mention L0S.
-                         */
-                        if (state & PCIE_LINK_STATE_L1)
-                                l1_latency += 1000;
-                }
+/**
+ * __pcie_aspm_check_state_one - check latency for endpoint device.
+ * @endpoint: pointer to the struct pci_dev of endpoint device
+ *
+ * TBD: The latency from the endpoint to root complex vary per switch's
+ * upstream link state above the device. Here we just do a simple check
+ * which assumes all links above the device can be in L1 state, that
+ * is we just consider the worst case. If switch's upstream link can't
+ * be put into L0S/L1, then our check is too strictly.
+ */
+static u32 __pcie_aspm_check_state_one(struct pci_dev *endpoint, u32 state)
+{
+        u32 l1_switch_latency = 0;
+        struct aspm_latency *acceptable;
+        struct pcie_link_state *link;
+
+        link = endpoint->bus->self->link_state;
+        state &= link->aspm_support;
+        acceptable = &link->acceptable[PCI_FUNC(endpoint->devfn)];
+
+        while (link && state) {
+                if ((state & PCIE_LINK_STATE_L0S) &&
+                    (link->latency.l0s > acceptable->l0s))
+                        state &= ~PCIE_LINK_STATE_L0S;
+                if ((state & PCIE_LINK_STATE_L1) &&
+                    (link->latency.l1 + l1_switch_latency > acceptable->l1))
+                        state &= ~PCIE_LINK_STATE_L1;
+                link = link->parent;
+                /*
+                 * Every switch on the path to root complex need 1
+                 * more microsecond for L1. Spec doesn't mention L0s.
+                 */
+                l1_switch_latency += 1000;
         }
         return state;
 }
 
-static unsigned int pcie_aspm_check_state(struct pci_dev *pdev,
-        unsigned int state)
+static u32 pcie_aspm_check_state(struct pcie_link_state *link, u32 state)
 {
-        struct pci_dev *child_dev;
+        pci_power_t power_state;
+        struct pci_dev *child;
+        struct pci_bus *linkbus = link->pdev->subordinate;
 
         /* If no child, ignore the link */
-        if (list_empty(&pdev->subordinate->devices))
+        if (list_empty(&linkbus->devices))
                 return state;
-        list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list) {
-                if (child_dev->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE) {
-                        /*
-                         * If downstream component of a link is pci bridge, we
-                         * disable ASPM for now for the link
-                         * */
-                        state = 0;
-                        break;
-                }
-                if ((child_dev->pcie_type != PCI_EXP_TYPE_ENDPOINT &&
-                        child_dev->pcie_type != PCI_EXP_TYPE_LEG_END))
+
+        list_for_each_entry(child, &linkbus->devices, bus_list) {
+                /*
+                 * If downstream component of a link is pci bridge, we
+                 * disable ASPM for now for the link
+                 */
+                if (child->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE)
+                        return 0;
+
+                if ((child->pcie_type != PCI_EXP_TYPE_ENDPOINT &&
+                     child->pcie_type != PCI_EXP_TYPE_LEG_END))
                         continue;
                 /* Device not in D0 doesn't need check latency */
-                if (child_dev->current_state == PCI_D1 ||
-                        child_dev->current_state == PCI_D2 ||
-                        child_dev->current_state == PCI_D3hot ||
-                        child_dev->current_state == PCI_D3cold)
+                power_state = child->current_state;
+                if (power_state == PCI_D1 || power_state == PCI_D2 ||
+                    power_state == PCI_D3hot || power_state == PCI_D3cold)
                         continue;
-                state = __pcie_aspm_check_state_one(child_dev, state);
+                state = __pcie_aspm_check_state_one(child, state);
         }
         return state;
 }
@@ -482,90 +453,71 @@ static void __pcie_aspm_config_one_dev(struct pci_dev *pdev, unsigned int state)
         pci_write_config_word(pdev, pos + PCI_EXP_LNKCTL, reg16);
 }
 
-static void __pcie_aspm_config_link(struct pci_dev *pdev, unsigned int state)
+static void __pcie_aspm_config_link(struct pcie_link_state *link, u32 state)
 {
-        struct pci_dev *child_dev;
-        int valid = 1;
-        struct pcie_link_state *link_state = pdev->link_state;
+        struct pci_dev *child, *parent = link->pdev;
+        struct pci_bus *linkbus = parent->subordinate;
 
         /* If no child, disable the link */
-        if (list_empty(&pdev->subordinate->devices))
+        if (list_empty(&linkbus->devices))
                 state = 0;
         /*
-         * if the downstream component has pci bridge function, don't do ASPM
-         * now
+         * If the downstream component has pci bridge function, don't
+         * do ASPM now.
          */
-        list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list) {
-                if (child_dev->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE) {
-                        valid = 0;
-                        break;
-                }
+        list_for_each_entry(child, &linkbus->devices, bus_list) {
+                if (child->pcie_type == PCI_EXP_TYPE_PCI_BRIDGE)
+                        return;
         }
-        if (!valid)
-                return;
-
         /*
-         * spec 2.0 suggests all functions should be configured the same
-         * setting for ASPM. Enabling ASPM L1 should be done in upstream
-         * component first and then downstream, and vice versa for disabling
-         * ASPM L1. Spec doesn't mention L0S.
+         * Spec 2.0 suggests all functions should be configured the
+         * same setting for ASPM. Enabling ASPM L1 should be done in
+         * upstream component first and then downstream, and vice
+         * versa for disabling ASPM L1. Spec doesn't mention L0S.
          */
         if (state & PCIE_LINK_STATE_L1)
-                __pcie_aspm_config_one_dev(pdev, state);
+                __pcie_aspm_config_one_dev(parent, state);
 
-        list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list)
-                __pcie_aspm_config_one_dev(child_dev, state);
+        list_for_each_entry(child, &linkbus->devices, bus_list)
+                __pcie_aspm_config_one_dev(child, state);
 
         if (!(state & PCIE_LINK_STATE_L1))
-                __pcie_aspm_config_one_dev(pdev, state);
+                __pcie_aspm_config_one_dev(parent, state);
 
-        link_state->enabled_state = state;
+        link->aspm_enabled = state;
 }
 
-static struct pcie_link_state *get_root_port_link(struct pcie_link_state *link)
+/* Check the whole hierarchy, and configure each link in the hierarchy */
+static void __pcie_aspm_configure_link_state(struct pcie_link_state *link,
+                                             u32 state)
 {
-        struct pcie_link_state *root_port_link = link;
-        while (root_port_link->parent)
-                root_port_link = root_port_link->parent;
-        return root_port_link;
-}
+        struct pcie_link_state *leaf, *root = link->root;
 
-/* check the whole hierarchy, and configure each link in the hierarchy */
-static void __pcie_aspm_configure_link_state(struct pci_dev *pdev,
-        unsigned int state)
-{
-        struct pcie_link_state *link_state = pdev->link_state;
-        struct pcie_link_state *root_port_link = get_root_port_link(link_state);
-        struct pcie_link_state *leaf;
+        state &= (PCIE_LINK_STATE_L0S | PCIE_LINK_STATE_L1);
 
-        state &= PCIE_LINK_STATE_L0S|PCIE_LINK_STATE_L1;
-
-        /* check all links who have specific root port link */
-        list_for_each_entry(leaf, &link_list, sibiling) {
-                if (!list_empty(&leaf->children) ||
-                        get_root_port_link(leaf) != root_port_link)
+        /* Check all links who have specific root port link */
+        list_for_each_entry(leaf, &link_list, sibling) {
+                if (!list_empty(&leaf->children) || (leaf->root != root))
                         continue;
-                state = pcie_aspm_check_state(leaf->pdev, state);
+                state = pcie_aspm_check_state(leaf, state);
         }
-        /* check root port link too in case it hasn't children */
-        state = pcie_aspm_check_state(root_port_link->pdev, state);
-
-        if (link_state->enabled_state == state)
+        /* Check root port link too in case it hasn't children */
+        state = pcie_aspm_check_state(root, state);
+        if (link->aspm_enabled == state)
                 return;
-
         /*
-         * we must change the hierarchy. See comments in
+         * We must change the hierarchy. See comments in
          * __pcie_aspm_config_link for the order
          **/
         if (state & PCIE_LINK_STATE_L1) {
-                list_for_each_entry(leaf, &link_list, sibiling) {
-                        if (get_root_port_link(leaf) == root_port_link)
-                                __pcie_aspm_config_link(leaf->pdev, state);
+                list_for_each_entry(leaf, &link_list, sibling) {
+                        if (leaf->root == root)
+                                __pcie_aspm_config_link(leaf, state);
                 }
         } else {
-                list_for_each_entry_reverse(leaf, &link_list, sibiling) {
-                        if (get_root_port_link(leaf) == root_port_link)
-                                __pcie_aspm_config_link(leaf->pdev, state);
+                list_for_each_entry_reverse(leaf, &link_list, sibling) {
+                        if (leaf->root == root)
+                                __pcie_aspm_config_link(leaf, state);
                 }
         }
 }
@@ -574,45 +526,42 @@ static void __pcie_aspm_configure_link_state(struct pci_dev *pdev,
  * pcie_aspm_configure_link_state: enable/disable PCI express link state
  * @pdev: the root port or switch downstream port
  */
-static void pcie_aspm_configure_link_state(struct pci_dev *pdev,
-        unsigned int state)
+static void pcie_aspm_configure_link_state(struct pcie_link_state *link,
+                                           u32 state)
 {
         down_read(&pci_bus_sem);
         mutex_lock(&aspm_lock);
-        __pcie_aspm_configure_link_state(pdev, state);
+        __pcie_aspm_configure_link_state(link, state);
         mutex_unlock(&aspm_lock);
         up_read(&pci_bus_sem);
 }
 
-static void free_link_state(struct pci_dev *pdev)
+static void free_link_state(struct pcie_link_state *link)
 {
-        kfree(pdev->link_state);
-        pdev->link_state = NULL;
+        link->pdev->link_state = NULL;
+        kfree(link);
 }
 
 static int pcie_aspm_sanity_check(struct pci_dev *pdev)
 {
-        struct pci_dev *child_dev;
-        int child_pos;
+        struct pci_dev *child;
+        int pos;
         u32 reg32;
-
         /*
-         * Some functions in a slot might not all be PCIE functions, very
-         * strange. Disable ASPM for the whole slot
+         * Some functions in a slot might not all be PCIE functions,
+         * very strange. Disable ASPM for the whole slot
          */
-        list_for_each_entry(child_dev, &pdev->subordinate->devices, bus_list) {
-                child_pos = pci_find_capability(child_dev, PCI_CAP_ID_EXP);
-                if (!child_pos)
+        list_for_each_entry(child, &pdev->subordinate->devices, bus_list) {
+                pos = pci_find_capability(child, PCI_CAP_ID_EXP);
+                if (!pos)
                         return -EINVAL;
-
                 /*
                  * Disable ASPM for pre-1.1 PCIe device, we follow MS to use
                  * RBER bit to determine if a function is 1.1 version device
                  */
-                pci_read_config_dword(child_dev, child_pos + PCI_EXP_DEVCAP,
-                        &reg32);
+                pci_read_config_dword(child, pos + PCI_EXP_DEVCAP, &reg32);
                 if (!(reg32 & PCI_EXP_DEVCAP_RBER) && !aspm_force) {
-                        dev_printk(KERN_INFO, &child_dev->dev, "disabling ASPM"
+                        dev_printk(KERN_INFO, &child->dev, "disabling ASPM"
                                 " on pre-1.1 PCIe device.  You can enable it"
                                 " with 'pcie_aspm=force'\n");
                         return -EINVAL;
@@ -621,6 +570,47 @@ static int pcie_aspm_sanity_check(struct pci_dev *pdev)
         return 0;
 }
 
+static struct pcie_link_state *pcie_aspm_setup_link_state(struct pci_dev *pdev)
+{
+        struct pcie_link_state *link;
+        int blacklist = !!pcie_aspm_sanity_check(pdev);
+
+        link = kzalloc(sizeof(*link), GFP_KERNEL);
+        if (!link)
+                return NULL;
+        INIT_LIST_HEAD(&link->sibling);
+        INIT_LIST_HEAD(&link->children);
+        INIT_LIST_HEAD(&link->link);
+        link->pdev = pdev;
+        if (pdev->pcie_type == PCI_EXP_TYPE_DOWNSTREAM) {
+                struct pcie_link_state *parent;
+                parent = pdev->bus->parent->self->link_state;
+                if (!parent) {
+                        kfree(link);
+                        return NULL;
+                }
+                link->parent = parent;
+                list_add(&link->link, &parent->children);
+        }
+        /* Setup a pointer to the root port link */
+        if (!link->parent)
+                link->root = link;
+        else
+                link->root = link->parent->root;
+
+        list_add(&link->sibling, &link_list);
+
+        pdev->link_state = link;
+
+        /* Check ASPM capability */
+        pcie_aspm_cap_init(link, blacklist);
+
+        /* Check Clock PM capability */
+        pcie_clkpm_cap_init(link, blacklist);
+
+        return link;
+}
+
 /*
  * pcie_aspm_init_link_state: Initiate PCI express link state.
  * It is called after the pcie and its children devices are scaned.
@@ -628,75 +618,47 @@ static int pcie_aspm_sanity_check(struct pci_dev *pdev)
  */
 void pcie_aspm_init_link_state(struct pci_dev *pdev)
 {
-        unsigned int state;
-        struct pcie_link_state *link_state;
-        int error = 0;
-        int blacklist;
+        u32 state;
+        struct pcie_link_state *link;
 
         if (aspm_disabled || !pdev->is_pcie || pdev->link_state)
                 return;
         if (pdev->pcie_type != PCI_EXP_TYPE_ROOT_PORT &&
-                pdev->pcie_type != PCI_EXP_TYPE_DOWNSTREAM)
+            pdev->pcie_type != PCI_EXP_TYPE_DOWNSTREAM)
+                return;
+
+        /* VIA has a strange chipset, root port is under a bridge */
+        if (pdev->pcie_type == PCI_EXP_TYPE_ROOT_PORT &&
+            pdev->bus->self)
                 return;
+
         down_read(&pci_bus_sem);
         if (list_empty(&pdev->subordinate->devices))
                 goto out;
 
-        blacklist = !!pcie_aspm_sanity_check(pdev);
-
         mutex_lock(&aspm_lock);
-
-        link_state = kzalloc(sizeof(*link_state), GFP_KERNEL);
-        if (!link_state)
-                goto unlock_out;
-
-        link_state->downstream_has_switch = pcie_aspm_downstream_has_switch(pdev);
-        INIT_LIST_HEAD(&link_state->children);
-        INIT_LIST_HEAD(&link_state->link);
-        if (pdev->bus->self) {/* this is a switch */
-                struct pcie_link_state *parent_link_state;
-
-                parent_link_state = pdev->bus->parent->self->link_state;
-                if (!parent_link_state) {
-                        kfree(link_state);
-                        goto unlock_out;
-                }
-                list_add(&link_state->link, &parent_link_state->children);
-                link_state->parent = parent_link_state;
-        }
-
-        pdev->link_state = link_state;
-
-        if (!blacklist) {
-                pcie_aspm_configure_common_clock(pdev);
-                pcie_aspm_cap_init(pdev);
+        link = pcie_aspm_setup_link_state(pdev);
+        if (!link)
+                goto unlock;
+        /*
+         * Setup initial ASPM state
+         *
+         * If link has switch, delay the link config. The leaf link
+         * initialization will config the whole hierarchy. But we must
+         * make sure BIOS doesn't set unsupported link state.
+         */
+        if (pcie_aspm_downstream_has_switch(link)) {
+                state = pcie_aspm_check_state(link, link->aspm_default);
+                __pcie_aspm_config_link(link, state);
         } else {
-                link_state->enabled_state = PCIE_LINK_STATE_L0S|PCIE_LINK_STATE_L1;
-                link_state->bios_aspm_state = 0;
-                /* Set support state to 0, so we will disable ASPM later */
-                link_state->support_state = 0;
+                state = policy_to_aspm_state(link);
+                __pcie_aspm_configure_link_state(link, state);
         }
 
-        link_state->pdev = pdev;
-        list_add(&link_state->sibiling, &link_list);
-
-        if (link_state->downstream_has_switch) {
-                /*
-                 * If link has switch, delay the link config. The leaf link
-                 * initialization will config the whole hierarchy. but we must
-                 * make sure BIOS doesn't set unsupported link state
-                 **/
-                state = pcie_aspm_check_state(pdev, link_state->bios_aspm_state);
-                __pcie_aspm_config_link(pdev, state);
-        } else
-                __pcie_aspm_configure_link_state(pdev,
-                        policy_to_aspm_state(pdev));
-
-        pcie_check_clock_pm(pdev, blacklist);
-
-unlock_out:
-        if (error)
-                free_link_state(pdev);
+        /* Setup initial Clock PM state */
+        state = (link->clkpm_capable) ? policy_to_clkpm_state(link) : 0;
+        pcie_set_clkpm(link, state);
+unlock:
         mutex_unlock(&aspm_lock);
 out:
         up_read(&pci_bus_sem);
@@ -725,11 +687,11 @@ void pcie_aspm_exit_link_state(struct pci_dev *pdev)
 
         /* All functions are removed, so just disable ASPM for the link */
         __pcie_aspm_config_one_dev(parent, 0);
-        list_del(&link_state->sibiling);
+        list_del(&link_state->sibling);
         list_del(&link_state->link);
         /* Clock PM is for endpoint device */
 
-        free_link_state(parent);
+        free_link_state(link_state);
 out:
         mutex_unlock(&aspm_lock);
         up_read(&pci_bus_sem);
@@ -749,7 +711,7 @@ void pcie_aspm_pm_state_change(struct pci_dev *pdev)
          * devices changed PM state, we should recheck if latency meets all
          * functions' requirement
          */
-        pcie_aspm_configure_link_state(pdev, link_state->enabled_state);
+        pcie_aspm_configure_link_state(link_state, link_state->aspm_enabled);
 }
 
 /*
@@ -772,14 +734,12 @@ void pci_disable_link_state(struct pci_dev *pdev, int state)
         down_read(&pci_bus_sem);
         mutex_lock(&aspm_lock);
         link_state = parent->link_state;
-        link_state->support_state &=
-                ~(state & (PCIE_LINK_STATE_L0S|PCIE_LINK_STATE_L1));
-        if (state & PCIE_LINK_STATE_CLKPM)
-                link_state->clk_pm_capable = 0;
-
-        __pcie_aspm_configure_link_state(parent, link_state->enabled_state);
-        if (!link_state->clk_pm_capable && link_state->clk_pm_enabled)
-                pcie_set_clock_pm(parent, 0);
+        link_state->aspm_support &= ~state;
+        __pcie_aspm_configure_link_state(link_state, link_state->aspm_enabled);
+        if (state & PCIE_LINK_STATE_CLKPM) {
+                link_state->clkpm_capable = 0;
+                pcie_set_clkpm(link_state, 0);
+        }
         mutex_unlock(&aspm_lock);
         up_read(&pci_bus_sem);
 }
@@ -788,7 +748,6 @@ EXPORT_SYMBOL(pci_disable_link_state);
 static int pcie_aspm_set_policy(const char *val, struct kernel_param *kp)
 {
         int i;
-        struct pci_dev *pdev;
         struct pcie_link_state *link_state;
 
         for (i = 0; i < ARRAY_SIZE(policy_str); i++)
@@ -802,14 +761,10 @@ static int pcie_aspm_set_policy(const char *val, struct kernel_param *kp)
         down_read(&pci_bus_sem);
         mutex_lock(&aspm_lock);
         aspm_policy = i;
-        list_for_each_entry(link_state, &link_list, sibiling) {
-                pdev = link_state->pdev;
-                __pcie_aspm_configure_link_state(pdev,
-                        policy_to_aspm_state(pdev));
-                if (link_state->clk_pm_capable &&
-                    link_state->clk_pm_enabled != policy_to_clkpm_state(pdev))
-                        pcie_set_clock_pm(pdev, policy_to_clkpm_state(pdev));
-
+        list_for_each_entry(link_state, &link_list, sibling) {
+                __pcie_aspm_configure_link_state(link_state,
+                        policy_to_aspm_state(link_state));
+                pcie_set_clkpm(link_state, policy_to_clkpm_state(link_state));
         }
         mutex_unlock(&aspm_lock);
         up_read(&pci_bus_sem);
@@ -838,7 +793,7 @@ static ssize_t link_state_show(struct device *dev,
         struct pci_dev *pci_device = to_pci_dev(dev);
         struct pcie_link_state *link_state = pci_device->link_state;
 
-        return sprintf(buf, "%d\n", link_state->enabled_state);
+        return sprintf(buf, "%d\n", link_state->aspm_enabled);
 }
 
 static ssize_t link_state_store(struct device *dev,
@@ -846,7 +801,7 @@ static ssize_t link_state_store(struct device *dev,
                 const char *buf,
                 size_t n)
 {
-        struct pci_dev *pci_device = to_pci_dev(dev);
+        struct pci_dev *pdev = to_pci_dev(dev);
         int state;
 
         if (n < 1)
@@ -854,7 +809,7 @@ static ssize_t link_state_store(struct device *dev,
         state = buf[0]-'0';
         if (state >= 0 && state <= 3) {
                 /* setup link aspm state */
-                pcie_aspm_configure_link_state(pci_device, state);
+                pcie_aspm_configure_link_state(pdev->link_state, state);
                 return n;
         }
 
@@ -868,7 +823,7 @@ static ssize_t clk_ctl_show(struct device *dev,
         struct pci_dev *pci_device = to_pci_dev(dev);
         struct pcie_link_state *link_state = pci_device->link_state;
 
-        return sprintf(buf, "%d\n", link_state->clk_pm_enabled);
+        return sprintf(buf, "%d\n", link_state->clkpm_enabled);
 }
 
 static ssize_t clk_ctl_store(struct device *dev,
@@ -876,7 +831,7 @@ static ssize_t clk_ctl_store(struct device *dev,
                 const char *buf,
                 size_t n)
 {
-        struct pci_dev *pci_device = to_pci_dev(dev);
+        struct pci_dev *pdev = to_pci_dev(dev);
         int state;
 
         if (n < 1)
@@ -885,7 +840,7 @@ static ssize_t clk_ctl_store(struct device *dev,
 
         down_read(&pci_bus_sem);
         mutex_lock(&aspm_lock);
-        pcie_set_clock_pm(pci_device, !!state);
+        pcie_set_clkpm_nocheck(pdev->link_state, !!state);
         mutex_unlock(&aspm_lock);
         up_read(&pci_bus_sem);
 
@@ -904,10 +859,10 @@ void pcie_aspm_create_sysfs_dev_files(struct pci_dev *pdev)
                 pdev->pcie_type != PCI_EXP_TYPE_DOWNSTREAM) || !link_state)
                 return;
 
-        if (link_state->support_state)
+        if (link_state->aspm_support)
                 sysfs_add_file_to_group(&pdev->dev.kobj,
                         &dev_attr_link_state.attr, power_group);
-        if (link_state->clk_pm_capable)
+        if (link_state->clkpm_capable)
                 sysfs_add_file_to_group(&pdev->dev.kobj,
                         &dev_attr_clk_ctl.attr, power_group);
 }
@@ -920,10 +875,10 @@ void pcie_aspm_remove_sysfs_dev_files(struct pci_dev *pdev)
                 pdev->pcie_type != PCI_EXP_TYPE_DOWNSTREAM) || !link_state)
                 return;
 
-        if (link_state->support_state)
+        if (link_state->aspm_support)
                 sysfs_remove_file_from_group(&pdev->dev.kobj,
                         &dev_attr_link_state.attr, power_group);
-        if (link_state->clk_pm_capable)
+        if (link_state->clkpm_capable)
                 sysfs_remove_file_from_group(&pdev->dev.kobj,
                         &dev_attr_clk_ctl.attr, power_group);
 }
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index f1ae2475ffff..40e75f6a5056 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -193,7 +193,7 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
                 res->flags |= pci_calc_resource_flags(l) | IORESOURCE_SIZEALIGN;
                 if (type == pci_bar_io) {
                         l &= PCI_BASE_ADDRESS_IO_MASK;
-                        mask = PCI_BASE_ADDRESS_IO_MASK & 0xffff;
+                        mask = PCI_BASE_ADDRESS_IO_MASK & IO_SPACE_LIMIT;
                 } else {
                         l &= PCI_BASE_ADDRESS_MEM_MASK;
                         mask = (u32)PCI_BASE_ADDRESS_MEM_MASK;
@@ -237,6 +237,8 @@ int __pci_read_base(struct pci_dev *dev, enum pci_bar_type type,
                         dev_printk(KERN_DEBUG, &dev->dev,
                                 "reg %x 64bit mmio: %pR\n", pos, res);
                 }
+
+                res->flags |= IORESOURCE_MEM_64;
         } else {
                 sz = pci_size(l, sz, mask);
 
@@ -287,7 +289,7 @@ void __devinit pci_read_bridge_bases(struct pci_bus *child)
         struct resource *res;
         int i;
 
-        if (!child->parent)     /* It's a host bus, nothing to read */
+        if (pci_is_root_bus(child))     /* It's a host bus, nothing to read */
                 return;
 
         if (dev->transparent) {
@@ -362,7 +364,10 @@ void __devinit pci_read_bridge_bases(struct pci_bus *child)
                 }
         }
         if (base <= limit) {
-                res->flags = (mem_base_lo & PCI_MEMORY_RANGE_TYPE_MASK) | IORESOURCE_MEM | IORESOURCE_PREFETCH;
+                res->flags = (mem_base_lo & PCI_PREF_RANGE_TYPE_MASK) |
+                                         IORESOURCE_MEM | IORESOURCE_PREFETCH;
+                if (res->flags & PCI_PREF_RANGE_TYPE_64)
+                        res->flags |= IORESOURCE_MEM_64;
                 res->start = base;
                 res->end = limit + 0xfffff;
                 dev_printk(KERN_DEBUG, &dev->dev, "bridge %sbit mmio pref: %pR\n",
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index bd4253f93d5a..56552d74abea 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -1133,6 +1133,7 @@ static void __init asus_hides_smbus_hostbridge(struct pci_dev *dev)
                         switch (dev->subsystem_device) {
                         case 0x1751: /* M2N notebook */
                         case 0x1821: /* M5N notebook */
+                        case 0x1897: /* A6L notebook */
                                 asus_hides_smbus = 1;
                         }
                 else if (dev->device == PCI_DEVICE_ID_INTEL_82855PM_HB)
@@ -1163,6 +1164,7 @@ static void __init asus_hides_smbus_hostbridge(struct pci_dev *dev)
                         switch (dev->subsystem_device) {
                         case 0x12bc: /* HP D330L */
                         case 0x12bd: /* HP D530 */
+                        case 0x006a: /* HP Compaq nx9500 */
                                 asus_hides_smbus = 1;
                         }
                 else if (dev->device == PCI_DEVICE_ID_INTEL_82875_HB)
@@ -2016,6 +2018,28 @@ DECLARE_PCI_FIXUP_FINAL(PCI_VENDOR_ID_BROADCOM,
                         PCI_DEVICE_ID_NX2_5709S,
                         quirk_brcm_570x_limit_vpd);
 
+/* Originally in EDAC sources for i82875P:
+ * Intel tells BIOS developers to hide device 6 which
+ * configures the overflow device access containing
+ * the DRBs - this is where we expose device 6.
+ * http://www.x86-secret.com/articles/tweak/pat/patsecrets-2.htm
+ */
+static void __devinit quirk_unhide_mch_dev6(struct pci_dev *dev)
+{
+        u8 reg;
+
+        if (pci_read_config_byte(dev, 0xF4, &reg) == 0 && !(reg & 0x02)) {
+                dev_info(&dev->dev, "Enabling MCH 'Overflow' Device\n");
+                pci_write_config_byte(dev, 0xF4, reg | 0x02);
+        }
+}
+
+DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_82865_HB,
+                        quirk_unhide_mch_dev6);
+DECLARE_PCI_FIXUP_EARLY(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_82875_HB,
+                        quirk_unhide_mch_dev6);
+
+
 #ifdef CONFIG_PCI_MSI
 /* Some chipsets do not support MSI. We cannot easily rely on setting
  * PCI_BUS_FLAGS_NO_MSI in its bus flags because there are actually
diff --git a/drivers/pci/remove.c b/drivers/pci/remove.c
index 86503c14ce7e..176615e7231f 100644
--- a/drivers/pci/remove.c
+++ b/drivers/pci/remove.c
@@ -32,8 +32,6 @@ static void pci_stop_dev(struct pci_dev *dev)
 
 static void pci_destroy_dev(struct pci_dev *dev)
 {
-        pci_stop_dev(dev);
-
         /* Remove the device from the device lists, and prevent any further
          * list accesses from this device */
         down_write(&pci_bus_sem);
diff --git a/drivers/pci/search.c b/drivers/pci/search.c
index 710d4ea69568..e8cb5051c311 100644
--- a/drivers/pci/search.c
+++ b/drivers/pci/search.c
@@ -29,7 +29,7 @@ pci_find_upstream_pcie_bridge(struct pci_dev *pdev)
         if (pdev->is_pcie)
                 return NULL;
         while (1) {
-                if (!pdev->bus->parent)
+                if (pci_is_root_bus(pdev->bus))
                         break;
                 pdev = pdev->bus->self;
                 /* a p2p bridge */
@@ -115,36 +115,6 @@ pci_find_next_bus(const struct pci_bus *from)
 
 #ifdef CONFIG_PCI_LEGACY
 /**
- * pci_find_slot - locate PCI device from a given PCI slot
- * @bus: number of PCI bus on which desired PCI device resides
- * @devfn: encodes number of PCI slot in which the desired PCI
- * device resides and the logical device number within that slot
- * in case of multi-function devices.
- *
- * Given a PCI bus and slot/function number, the desired PCI device
- * is located in system global list of PCI devices.  If the device
- * is found, a pointer to its data structure is returned.  If no
- * device is found, %NULL is returned.
- *
- * NOTE: Do not use this function any more; use pci_get_slot() instead, as
- * the PCI device returned by this function can disappear at any moment in
- * time.
- */
-struct pci_dev *pci_find_slot(unsigned int bus, unsigned int devfn)
-{
-        struct pci_dev *dev = NULL;
-
-        while ((dev = pci_get_device(PCI_ANY_ID, PCI_ANY_ID, dev)) != NULL) {
-                if (dev->bus->number == bus && dev->devfn == devfn) {
-                        pci_dev_put(dev);
-                        return dev;
-                }
-        }
-        return NULL;
-}
-EXPORT_SYMBOL(pci_find_slot);
-
-/**
  * pci_find_device - begin or continue searching for a PCI device by vendor/device id
  * @vendor: PCI vendor id to match, or %PCI_ANY_ID to match all vendor ids
  * @device: PCI device id to match, or %PCI_ANY_ID to match all device ids
diff --git a/drivers/pci/setup-bus.c b/drivers/pci/setup-bus.c
index a00f85471b6e..b636e245445d 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -58,7 +58,6 @@ static void pbus_assign_resources_sorted(const struct pci_bus *bus)
                 res = list->res;
                 idx = res - &list->dev->resource[0];
                 if (pci_assign_resource(list->dev, idx)) {
-                        /* FIXME: get rid of this */
                         res->start = 0;
                         res->end = 0;
                         res->flags = 0;
@@ -143,6 +142,7 @@ static void pci_setup_bridge(struct pci_bus *bus)
         struct pci_dev *bridge = bus->self;
         struct pci_bus_region region;
         u32 l, bu, lu, io_upper16;
+        int pref_mem64;
 
         if (pci_is_enabled(bridge))
                 return;
@@ -198,16 +198,22 @@ static void pci_setup_bridge(struct pci_bus *bus)
         pci_write_config_dword(bridge, PCI_PREF_LIMIT_UPPER32, 0);
 
         /* Set up PREF base/limit. */
+        pref_mem64 = 0;
         bu = lu = 0;
         pcibios_resource_to_bus(bridge, &region, bus->resource[2]);
         if (bus->resource[2]->flags & IORESOURCE_PREFETCH) {
+                int width = 8;
                 l = (region.start >> 16) & 0xfff0;
                 l |= region.end & 0xfff00000;
-                bu = upper_32_bits(region.start);
-                lu = upper_32_bits(region.end);
-                dev_info(&bridge->dev, "  PREFETCH window: %#016llx-%#016llx\n",
-                    (unsigned long long)region.start,
-                    (unsigned long long)region.end);
+                if (bus->resource[2]->flags & IORESOURCE_MEM_64) {
+                        pref_mem64 = 1;
+                        bu = upper_32_bits(region.start);
+                        lu = upper_32_bits(region.end);
+                        width = 16;
+                }
+                dev_info(&bridge->dev, "  PREFETCH window: %#0*llx-%#0*llx\n",
+                                width, (unsigned long long)region.start,
+                                width, (unsigned long long)region.end);
         }
         else {
                 l = 0x0000fff0;
@@ -215,9 +221,11 @@ static void pci_setup_bridge(struct pci_bus *bus)
         }
         pci_write_config_dword(bridge, PCI_PREF_MEMORY_BASE, l);
 
-        /* Set the upper 32 bits of PREF base & limit. */
-        pci_write_config_dword(bridge, PCI_PREF_BASE_UPPER32, bu);
-        pci_write_config_dword(bridge, PCI_PREF_LIMIT_UPPER32, lu);
+        if (pref_mem64) {
+                /* Set the upper 32 bits of PREF base & limit. */
+                pci_write_config_dword(bridge, PCI_PREF_BASE_UPPER32, bu);
+                pci_write_config_dword(bridge, PCI_PREF_LIMIT_UPPER32, lu);
+        }
 
         pci_write_config_word(bridge, PCI_BRIDGE_CONTROL, bus->bridge_ctl);
 }
@@ -255,8 +263,25 @@ static void pci_bridge_check_ranges(struct pci_bus *bus)
                 pci_read_config_dword(bridge, PCI_PREF_MEMORY_BASE, &pmem);
                 pci_write_config_dword(bridge, PCI_PREF_MEMORY_BASE, 0x0);
         }
-        if (pmem)
+        if (pmem) {
                 b_res[2].flags |= IORESOURCE_MEM | IORESOURCE_PREFETCH;
+                if ((pmem & PCI_PREF_RANGE_TYPE_MASK) == PCI_PREF_RANGE_TYPE_64)
+                        b_res[2].flags |= IORESOURCE_MEM_64;
+        }
+
+        /* double check if bridge does support 64 bit pref */
+        if (b_res[2].flags & IORESOURCE_MEM_64) {
+                u32 mem_base_hi, tmp;
+                pci_read_config_dword(bridge, PCI_PREF_BASE_UPPER32,
+                                         &mem_base_hi);
+                pci_write_config_dword(bridge, PCI_PREF_BASE_UPPER32,
+                                               0xffffffff);
+                pci_read_config_dword(bridge, PCI_PREF_BASE_UPPER32, &tmp);
+                if (!tmp)
+                        b_res[2].flags &= ~IORESOURCE_MEM_64;
+                pci_write_config_dword(bridge, PCI_PREF_BASE_UPPER32,
+                                       mem_base_hi);
+        }
 }
 
 /* Helper function for sizing routines: find first available
@@ -336,6 +361,7 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask, unsigned long
         resource_size_t aligns[12];     /* Alignments from 1Mb to 2Gb */
         int order, max_order;
         struct resource *b_res = find_free_bus_resource(bus, type);
+        unsigned int mem64_mask = 0;
 
         if (!b_res)
                 return 0;
@@ -344,9 +370,12 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask, unsigned long
         max_order = 0;
         size = 0;
 
+        mem64_mask = b_res->flags & IORESOURCE_MEM_64;
+        b_res->flags &= ~IORESOURCE_MEM_64;
+
         list_for_each_entry(dev, &bus->devices, bus_list) {
                 int i;
-                
+
                 for (i = 0; i < PCI_NUM_RESOURCES; i++) {
                         struct resource *r = &dev->resource[i];
                         resource_size_t r_size;
@@ -372,6 +401,7 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask, unsigned long
                                 aligns[order] += align;
                         if (order > max_order)
                                 max_order = order;
+                        mem64_mask &= r->flags & IORESOURCE_MEM_64;
                 }
         }
 
@@ -396,6 +426,7 @@ static int pbus_size_mem(struct pci_bus *bus, unsigned long mask, unsigned long
         b_res->start = min_align;
         b_res->end = size + min_align - 1;
         b_res->flags |= IORESOURCE_STARTALIGN;
+        b_res->flags |= mem64_mask;
         return 1;
 }
 
diff --git a/drivers/pci/setup-res.c b/drivers/pci/setup-res.c
index 12403516776a..b711fb7181e2 100644
--- a/drivers/pci/setup-res.c
+++ b/drivers/pci/setup-res.c
@@ -135,23 +135,16 @@ void pci_disable_bridge_window(struct pci_dev *dev)
 }
 #endif  /* CONFIG_PCI_QUIRKS */
 
-int pci_assign_resource(struct pci_dev *dev, int resno)
+static int __pci_assign_resource(struct pci_bus *bus, struct pci_dev *dev,
+                                 int resno)
 {
-        struct pci_bus *bus = dev->bus;
         struct resource *res = dev->resource + resno;
         resource_size_t size, min, align;
         int ret;
 
         size = resource_size(res);
         min = (res->flags & IORESOURCE_IO) ? PCIBIOS_MIN_IO : PCIBIOS_MIN_MEM;
-
         align = resource_alignment(res);
-        if (!align) {
-                dev_info(&dev->dev, "BAR %d: can't allocate resource (bogus "
-                        "alignment) %pR flags %#lx\n",
-                        resno, res, res->flags);
-                return -EINVAL;
-        }
 
         /* First, try exact prefetching match.. */
         ret = pci_bus_alloc_resource(bus, res, size, align, min,
@@ -169,10 +162,7 @@ int pci_assign_resource(struct pci_dev *dev, int resno)
                                              pcibios_align_resource, dev);
         }
 
-        if (ret) {
-                dev_info(&dev->dev, "BAR %d: can't allocate %s resource %pR\n",
-                        resno, res->flags & IORESOURCE_IO ? "I/O" : "mem", res);
-        } else {
+        if (!ret) {
                 res->flags &= ~IORESOURCE_STARTALIGN;
                 if (resno < PCI_BRIDGE_RESOURCES)
                         pci_update_resource(dev, resno);
@@ -181,6 +171,39 @@ int pci_assign_resource(struct pci_dev *dev, int resno)
         return ret;
 }
 
+int pci_assign_resource(struct pci_dev *dev, int resno)
+{
+        struct resource *res = dev->resource + resno;
+        resource_size_t align;
+        struct pci_bus *bus;
+        int ret;
+
+        align = resource_alignment(res);
+        if (!align) {
+                dev_info(&dev->dev, "BAR %d: can't allocate resource (bogus "
+                        "alignment) %pR flags %#lx\n",
+                        resno, res, res->flags);
+                return -EINVAL;
+        }
+
+        bus = dev->bus;
+        while ((ret = __pci_assign_resource(bus, dev, resno))) {
+                if (bus->parent && bus->self->transparent)
+                        bus = bus->parent;
+                else
+                        bus = NULL;
+                if (bus)
+                        continue;
+                break;
+        }
+
+        if (ret)
+                dev_info(&dev->dev, "BAR %d: can't allocate %s resource %pR\n",
+                        resno, res->flags & IORESOURCE_IO ? "I/O" : "mem", res);
+
+        return ret;
+}
+
 #if 0
 int pci_assign_resource_fixed(struct pci_dev *dev, int resno)
 {
diff --git a/drivers/pci/slot.c b/drivers/pci/slot.c
index fe95ce20bcbd..eddb0748b0ea 100644
--- a/drivers/pci/slot.c
+++ b/drivers/pci/slot.c
@@ -307,6 +307,45 @@ void pci_destroy_slot(struct pci_slot *slot)
 }
 EXPORT_SYMBOL_GPL(pci_destroy_slot);
 
+#if defined(CONFIG_HOTPLUG_PCI) || defined(CONFIG_HOTPLUG_PCI_MODULE)
+#include <linux/pci_hotplug.h>
+/**
+ * pci_hp_create_link - create symbolic link to the hotplug driver module.
+ * @slot: struct pci_slot
+ *
+ * Helper function for pci_hotplug_core.c to create symbolic link to
+ * the hotplug driver module.
+ */
+void pci_hp_create_module_link(struct pci_slot *pci_slot)
+{
+        struct hotplug_slot *slot = pci_slot->hotplug;
+        struct kobject *kobj = NULL;
+        int no_warn;
+
+        if (!slot || !slot->ops)
+                return;
+        kobj = kset_find_obj(module_kset, slot->ops->mod_name);
+        if (!kobj)
+                return;
+        no_warn = sysfs_create_link(&pci_slot->kobj, kobj, "module");
+        kobject_put(kobj);
+}
+EXPORT_SYMBOL_GPL(pci_hp_create_module_link);
+
+/**
+ * pci_hp_remove_link - remove symbolic link to the hotplug driver module.
+ * @slot: struct pci_slot
+ *
+ * Helper function for pci_hotplug_core.c to remove symbolic link to
+ * the hotplug driver module.
+ */
+void pci_hp_remove_module_link(struct pci_slot *pci_slot)
+{
+        sysfs_remove_link(&pci_slot->kobj, "module");
+}
+EXPORT_SYMBOL_GPL(pci_hp_remove_module_link);
+#endif
+
 static int pci_slot_init(void)
 {
         struct kset *pci_bus_kset;
diff --git a/drivers/platform/x86/acer-wmi.c b/drivers/platform/x86/acer-wmi.c
index 09a503e5da6a..be2fd6f91639 100644
--- a/drivers/platform/x86/acer-wmi.c
+++ b/drivers/platform/x86/acer-wmi.c
@@ -958,12 +958,12 @@ static void acer_rfkill_update(struct work_struct *ignored)
 
         status = get_u32(&state, ACER_CAP_WIRELESS);
         if (ACPI_SUCCESS(status))
-                rfkill_set_sw_state(wireless_rfkill, !!state);
+                rfkill_set_sw_state(wireless_rfkill, !state);
 
         if (has_cap(ACER_CAP_BLUETOOTH)) {
                 status = get_u32(&state, ACER_CAP_BLUETOOTH);
                 if (ACPI_SUCCESS(status))
-                        rfkill_set_sw_state(bluetooth_rfkill, !!state);
+                        rfkill_set_sw_state(bluetooth_rfkill, !state);
         }
 
         schedule_delayed_work(&acer_rfkill_work, round_jiffies_relative(HZ));
diff --git a/drivers/platform/x86/eeepc-laptop.c b/drivers/platform/x86/eeepc-laptop.c
index 03bf522bd7ab..8153b3e59189 100644
--- a/drivers/platform/x86/eeepc-laptop.c
+++ b/drivers/platform/x86/eeepc-laptop.c
@@ -180,6 +180,7 @@ static struct key_entry eeepc_keymap[] = {
  */
 static int eeepc_hotk_add(struct acpi_device *device);
 static int eeepc_hotk_remove(struct acpi_device *device, int type);
+static int eeepc_hotk_resume(struct acpi_device *device);
 
 static const struct acpi_device_id eeepc_device_ids[] = {
         {EEEPC_HOTK_HID, 0},
@@ -194,6 +195,7 @@ static struct acpi_driver eeepc_hotk_driver = {
         .ops = {
                 .add = eeepc_hotk_add,
                 .remove = eeepc_hotk_remove,
+                .resume = eeepc_hotk_resume,
         },
 };
 
@@ -512,15 +514,12 @@ static int notify_brn(void)
         return -1;
 }
 
-static void eeepc_rfkill_notify(acpi_handle handle, u32 event, void *data)
+static void eeepc_rfkill_hotplug(void)
 {
         struct pci_dev *dev;
         struct pci_bus *bus = pci_find_bus(0, 1);
         bool blocked;
 
-        if (event != ACPI_NOTIFY_BUS_CHECK)
-                return;
-
         if (!bus) {
                 printk(EEEPC_WARNING "Unable to find PCI bus 1?\n");
                 return;
@@ -551,6 +550,14 @@ static void eeepc_rfkill_notify(acpi_handle handle, u32 event, void *data)
         rfkill_set_sw_state(ehotk->eeepc_wlan_rfkill, blocked);
 }
 
+static void eeepc_rfkill_notify(acpi_handle handle, u32 event, void *data)
+{
+        if (event != ACPI_NOTIFY_BUS_CHECK)
+                return;
+
+        eeepc_rfkill_hotplug();
+}
+
 static void eeepc_hotk_notify(acpi_handle handle, u32 event, void *data)
 {
         static struct key_entry *key;
@@ -675,8 +682,8 @@ static int eeepc_hotk_add(struct acpi_device *device)
                 if (!ehotk->eeepc_wlan_rfkill)
                         goto wlan_fail;
 
-                rfkill_set_sw_state(ehotk->eeepc_wlan_rfkill,
-                                    get_acpi(CM_ASL_WLAN) != 1);
+                rfkill_init_sw_state(ehotk->eeepc_wlan_rfkill,
+                                     get_acpi(CM_ASL_WLAN) != 1);
                 result = rfkill_register(ehotk->eeepc_wlan_rfkill);
                 if (result)
                         goto wlan_fail;
@@ -693,8 +700,8 @@ static int eeepc_hotk_add(struct acpi_device *device)
                 if (!ehotk->eeepc_bluetooth_rfkill)
                         goto bluetooth_fail;
 
-                rfkill_set_sw_state(ehotk->eeepc_bluetooth_rfkill,
-                                    get_acpi(CM_ASL_BLUETOOTH) != 1);
+                rfkill_init_sw_state(ehotk->eeepc_bluetooth_rfkill,
+                                     get_acpi(CM_ASL_BLUETOOTH) != 1);
                 result = rfkill_register(ehotk->eeepc_bluetooth_rfkill);
                 if (result)
                         goto bluetooth_fail;
@@ -734,6 +741,33 @@ static int eeepc_hotk_remove(struct acpi_device *device, int type)
         return 0;
 }
 
+static int eeepc_hotk_resume(struct acpi_device *device)
+{
+        if (ehotk->eeepc_wlan_rfkill) {
+                bool wlan;
+
+                /* Workaround - it seems that _PTS disables the wireless
+                   without notification or changing the value read by WLAN.
+                   Normally this is fine because the correct value is restored
+                   from the non-volatile storage on resume, but we need to do
+                   it ourself if case suspend is aborted, or we lose wireless.
+                 */
+                wlan = get_acpi(CM_ASL_WLAN);
+                set_acpi(CM_ASL_WLAN, wlan);
+
+                rfkill_set_sw_state(ehotk->eeepc_wlan_rfkill,
+                                    wlan != 1);
+
+                eeepc_rfkill_hotplug();
+        }
+
+        if (ehotk->eeepc_bluetooth_rfkill)
+                rfkill_set_sw_state(ehotk->eeepc_bluetooth_rfkill,
+                                    get_acpi(CM_ASL_BLUETOOTH) != 1);
+
+        return 0;
+}
+
 /*
  * Hwmon
  */
diff --git a/drivers/platform/x86/thinkpad_acpi.c b/drivers/platform/x86/thinkpad_acpi.c
index 86e958539f46..40d64c03278c 100644
--- a/drivers/platform/x86/thinkpad_acpi.c
+++ b/drivers/platform/x86/thinkpad_acpi.c
@@ -1163,8 +1163,8 @@ static int __init tpacpi_new_rfkill(const enum tpacpi_rfk_id id,
 {
         struct tpacpi_rfk *atp_rfk;
         int res;
-        bool initial_sw_state = false;
-        int initial_sw_status;
+        bool sw_state = false;
+        int sw_status;
 
         BUG_ON(id >= TPACPI_RFK_SW_MAX || tpacpi_rfkill_switches[id]);
 
@@ -1185,17 +1185,17 @@ static int __init tpacpi_new_rfkill(const enum tpacpi_rfk_id id,
         atp_rfk->id = id;
         atp_rfk->ops = tp_rfkops;
 
-        initial_sw_status = (tp_rfkops->get_status)();
-        if (initial_sw_status < 0) {
+        sw_status = (tp_rfkops->get_status)();
+        if (sw_status < 0) {
                 printk(TPACPI_ERR
                         "failed to read initial state for %s, error %d\n",
-                        name, initial_sw_status);
+                        name, sw_status);
         } else {
-                initial_sw_state = (initial_sw_status == TPACPI_RFK_RADIO_OFF);
+                sw_state = (sw_status == TPACPI_RFK_RADIO_OFF);
                 if (set_default) {
                         /* try to keep the initial state, since we ask the
                          * firmware to preserve it across S5 in NVRAM */
-                        rfkill_set_sw_state(atp_rfk->rfkill, initial_sw_state);
+                        rfkill_init_sw_state(atp_rfk->rfkill, sw_state);
                 }
         }
         rfkill_set_hw_state(atp_rfk->rfkill, tpacpi_rfk_check_hwblock_state());
diff --git a/drivers/s390/block/dasd.c b/drivers/s390/block/dasd.c
index e5b84db0aa03..749836668655 100644
--- a/drivers/s390/block/dasd.c
+++ b/drivers/s390/block/dasd.c
@@ -470,7 +470,7 @@ static int dasd_decrease_state(struct dasd_device *device)
  */
 static void dasd_change_state(struct dasd_device *device)
 {
-        int rc;
+        int rc;
 
         if (device->state == device->target)
                 /* Already where we want to go today... */
@@ -479,8 +479,10 @@ static void dasd_change_state(struct dasd_device *device)
                 rc = dasd_increase_state(device);
         else
                 rc = dasd_decrease_state(device);
-        if (rc && rc != -EAGAIN)
-                device->target = device->state;
+        if (rc == -EAGAIN)
+                return;
+        if (rc)
+                device->target = device->state;
 
         if (device->state == device->target) {
                 wake_up(&dasd_init_waitq);
@@ -2503,15 +2505,25 @@ int dasd_generic_restore_device(struct ccw_device *cdev)
         if (IS_ERR(device))
                 return PTR_ERR(device);
 
+        /* allow new IO again */
+        device->stopped &= ~DASD_STOPPED_PM;
+        device->stopped &= ~DASD_UNRESUMED_PM;
+
         dasd_schedule_device_bh(device);
         if (device->block)
                 dasd_schedule_block_bh(device->block);
 
         if (device->discipline->restore)
                 rc = device->discipline->restore(device);
+        if (rc)
+                /*
+                 * if the resume failed for the DASD we put it in
+                 * an UNRESUMED stop state
+                 */
+                device->stopped |= DASD_UNRESUMED_PM;
 
         dasd_put_device(device);
-        return rc;
+        return 0;
 }
 EXPORT_SYMBOL_GPL(dasd_generic_restore_device);
 
diff --git a/drivers/s390/block/dasd_eckd.c b/drivers/s390/block/dasd_eckd.c
index 1c28ec3e4ccb..f8b1f04f26b8 100644
--- a/drivers/s390/block/dasd_eckd.c
+++ b/drivers/s390/block/dasd_eckd.c
@@ -3243,9 +3243,6 @@ int dasd_eckd_restore_device(struct dasd_device *device)
         int is_known, rc;
         struct dasd_uid temp_uid;
 
-        /* allow new IO again */
-        device->stopped &= ~DASD_STOPPED_PM;
-
         private = (struct dasd_eckd_private *) device->private;
 
         /* Read Configuration Data */
@@ -3295,12 +3292,7 @@ int dasd_eckd_restore_device(struct dasd_device *device)
         return 0;
 
 out_err:
-        /*
-         * if the resume failed for the DASD we put it in
-         * an UNRESUMED stop state
-         */
-        device->stopped |= DASD_UNRESUMED_PM;
-        return 0;
+        return -1;
 }
 
 static struct ccw_driver dasd_eckd_driver = {
diff --git a/drivers/s390/char/con3215.c b/drivers/s390/char/con3215.c
index 04dc734805c6..21639d6c996f 100644
--- a/drivers/s390/char/con3215.c
+++ b/drivers/s390/char/con3215.c
@@ -20,10 +20,7 @@
 #include <linux/interrupt.h>
 #include <linux/err.h>
 #include <linux/reboot.h>
-
 #include <linux/slab.h>
-#include <linux/bootmem.h>
-
 #include <asm/ccwdev.h>
 #include <asm/cio.h>
 #include <asm/io.h>
@@ -735,7 +732,7 @@ static int raw3215_pm_stop(struct ccw_device *cdev)
         unsigned long flags;
 
         /* Empty the output buffer, then prevent new I/O. */
-        raw = cdev->dev.driver_data;
+        raw = dev_get_drvdata(&cdev->dev);
         spin_lock_irqsave(get_ccwdev_lock(raw->cdev), flags);
         raw3215_make_room(raw, RAW3215_BUFFER_SIZE);
         raw->flags |= RAW3215_FROZEN;
@@ -749,7 +746,7 @@ static int raw3215_pm_start(struct ccw_device *cdev)
         unsigned long flags;
 
         /* Allow I/O again and flush output buffer. */
-        raw = cdev->dev.driver_data;
+        raw = dev_get_drvdata(&cdev->dev);
         spin_lock_irqsave(get_ccwdev_lock(raw->cdev), flags);
         raw->flags &= ~RAW3215_FROZEN;
         raw->flags |= RAW3215_FLUSHING;
@@ -883,7 +880,7 @@ static int __init con3215_init(void)
         raw3215_freelist = NULL;
         spin_lock_init(&raw3215_freelist_lock);
         for (i = 0; i < NR_3215_REQ; i++) {
-                req = (struct raw3215_req *) alloc_bootmem_low(sizeof(struct raw3215_req));
+                req = kzalloc(sizeof(struct raw3215_req), GFP_KERNEL | GFP_DMA);
                 req->next = raw3215_freelist;
                 raw3215_freelist = req;
         }
@@ -893,10 +890,9 @@ static int __init con3215_init(void)
                 return -ENODEV;
 
         raw3215[0] = raw = (struct raw3215_info *)
-                alloc_bootmem_low(sizeof(struct raw3215_info));
-        memset(raw, 0, sizeof(struct raw3215_info));
-        raw->buffer = (char *) alloc_bootmem_low(RAW3215_BUFFER_SIZE);
-        raw->inbuf = (char *) alloc_bootmem_low(RAW3215_INBUF_SIZE);
+                kzalloc(sizeof(struct raw3215_info), GFP_KERNEL | GFP_DMA);
+        raw->buffer = kzalloc(RAW3215_BUFFER_SIZE, GFP_KERNEL | GFP_DMA);
+        raw->inbuf = kzalloc(RAW3215_INBUF_SIZE, GFP_KERNEL | GFP_DMA);
         raw->cdev = cdev;
         dev_set_drvdata(&cdev->dev, raw);
         cdev->handler = raw3215_irq;
@@ -906,9 +902,9 @@ static int __init con3215_init(void)
 
         /* Request the console irq */
         if (raw3215_startup(raw) != 0) {
-                free_bootmem((unsigned long) raw->inbuf, RAW3215_INBUF_SIZE);
-                free_bootmem((unsigned long) raw->buffer, RAW3215_BUFFER_SIZE);
-                free_bootmem((unsigned long) raw, sizeof(struct raw3215_info));
+                kfree(raw->inbuf);
+                kfree(raw->buffer);
+                kfree(raw);
                 raw3215[0] = NULL;
                 return -ENODEV;
         }
diff --git a/drivers/s390/char/con3270.c b/drivers/s390/char/con3270.c
index 44d02e371c04..bb838bdf829d 100644
--- a/drivers/s390/char/con3270.c
+++ b/drivers/s390/char/con3270.c
@@ -7,7 +7,6 @@
  *     Copyright IBM Corp. 2003, 2009
  */
 
-#include <linux/bootmem.h>
 #include <linux/console.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
@@ -600,16 +599,14 @@ con3270_init(void)
         if (IS_ERR(rp))
                 return PTR_ERR(rp);
 
-        condev = (struct con3270 *) alloc_bootmem_low(sizeof(struct con3270));
-        memset(condev, 0, sizeof(struct con3270));
+        condev = kzalloc(sizeof(struct con3270), GFP_KERNEL | GFP_DMA);
         condev->view.dev = rp;
 
-        condev->read = raw3270_request_alloc_bootmem(0);
+        condev->read = raw3270_request_alloc(0);
         condev->read->callback = con3270_read_callback;
         condev->read->callback_data = condev;
-        condev->write = 
-                raw3270_request_alloc_bootmem(CON3270_OUTPUT_BUFFER_SIZE);
-        condev->kreset = raw3270_request_alloc_bootmem(1);
+        condev->write = raw3270_request_alloc(CON3270_OUTPUT_BUFFER_SIZE);
+        condev->kreset = raw3270_request_alloc(1);
 
         INIT_LIST_HEAD(&condev->lines);
         INIT_LIST_HEAD(&condev->update);
@@ -623,7 +620,7 @@ con3270_init(void)
 
         INIT_LIST_HEAD(&condev->freemem);
         for (i = 0; i < CON3270_STRING_PAGES; i++) {
-                cbuf = (void *) alloc_bootmem_low_pages(PAGE_SIZE);
+                cbuf = (void *) get_zeroed_page(GFP_KERNEL | GFP_DMA);
                 add_string_memory(&condev->freemem, cbuf, PAGE_SIZE);
         }
         condev->cline = alloc_string(&condev->freemem, condev->view.cols);
diff --git a/drivers/s390/char/monreader.c b/drivers/s390/char/monreader.c
index 75a8831eebbc..7892550d7932 100644
--- a/drivers/s390/char/monreader.c
+++ b/drivers/s390/char/monreader.c
@@ -320,7 +320,7 @@ static int mon_open(struct inode *inode, struct file *filp)
                 goto out_path;
         }
         filp->private_data = monpriv;
-        monreader_device->driver_data = monpriv;
+        dev_set_drvdata(&monreader_device, monpriv);
         unlock_kernel();
         return nonseekable_open(inode, filp);
 
@@ -463,7 +463,7 @@ static struct miscdevice mon_dev = {
  *****************************************************************************/
 static int monreader_freeze(struct device *dev)
 {
-        struct mon_private *monpriv = dev->driver_data;
+        struct mon_private *monpriv = dev_get_drvdata(&dev);
         int rc;
 
         if (!monpriv)
@@ -487,7 +487,7 @@ static int monreader_freeze(struct device *dev)
 
 static int monreader_thaw(struct device *dev)
 {
-        struct mon_private *monpriv = dev->driver_data;
+        struct mon_private *monpriv = dev_get_drvdata(dev);
         int rc;
 
         if (!monpriv)
diff --git a/drivers/s390/char/raw3270.c b/drivers/s390/char/raw3270.c
index acab7b2dfe8a..d6a022f55e92 100644
--- a/drivers/s390/char/raw3270.c
+++ b/drivers/s390/char/raw3270.c
@@ -7,7 +7,6 @@
  *     Copyright IBM Corp. 2003, 2009
  */
 
-#include <linux/bootmem.h>
 #include <linux/module.h>
 #include <linux/err.h>
 #include <linux/init.h>
@@ -143,33 +142,6 @@ raw3270_request_alloc(size_t size)
         return rq;
 }
 
-#ifdef CONFIG_TN3270_CONSOLE
-/*
- * Allocate a new 3270 ccw request from bootmem. Only works very
- * early in the boot process. Only con3270.c should be using this.
- */
-struct raw3270_request __init *raw3270_request_alloc_bootmem(size_t size)
-{
-        struct raw3270_request *rq;
-
-        rq = alloc_bootmem_low(sizeof(struct raw3270));
-
-        /* alloc output buffer. */
-        if (size > 0)
-                rq->buffer = alloc_bootmem_low(size);
-        rq->size = size;
-        INIT_LIST_HEAD(&rq->list);
-
-        /*
-         * Setup ccw.
-         */
-        rq->ccw.cda = __pa(rq->buffer);
-        rq->ccw.flags = CCW_FLAG_SLI;
-
-        return rq;
-}
-#endif
-
 /*
  * Free 3270 ccw request
  */
@@ -846,8 +818,8 @@ struct raw3270 __init *raw3270_setup_console(struct ccw_device *cdev)
         char *ascebc;
         int rc;
 
-        rp = (struct raw3270 *) alloc_bootmem_low(sizeof(struct raw3270));
-        ascebc = (char *) alloc_bootmem(256);
+        rp = kzalloc(sizeof(struct raw3270), GFP_KERNEL | GFP_DMA);
+        ascebc = kzalloc(256, GFP_KERNEL);
         rc = raw3270_setup_device(cdev, rp, ascebc);
         if (rc)
                 return ERR_PTR(rc);
@@ -1350,7 +1322,7 @@ static int raw3270_pm_stop(struct ccw_device *cdev)
         struct raw3270_view *view;
         unsigned long flags;
 
-        rp = cdev->dev.driver_data;
+        rp = dev_get_drvdata(&cdev->dev);
         if (!rp)
                 return 0;
         spin_lock_irqsave(get_ccwdev_lock(rp->cdev), flags);
@@ -1376,7 +1348,7 @@ static int raw3270_pm_start(struct ccw_device *cdev)
         struct raw3270 *rp;
         unsigned long flags;
 
-        rp = cdev->dev.driver_data;
+        rp = dev_get_drvdata(&cdev->dev);
         if (!rp)
                 return 0;
         spin_lock_irqsave(get_ccwdev_lock(rp->cdev), flags);
diff --git a/drivers/s390/char/sclp_con.c b/drivers/s390/char/sclp_con.c
index 336811a77672..ad698d30cb3b 100644
--- a/drivers/s390/char/sclp_con.c
+++ b/drivers/s390/char/sclp_con.c
@@ -11,7 +11,6 @@
 #include <linux/init.h>
 #include <linux/timer.h>
 #include <linux/jiffies.h>
-#include <linux/bootmem.h>
 #include <linux/termios.h>
 #include <linux/err.h>
 #include <linux/reboot.h>
@@ -110,7 +109,7 @@ static void sclp_console_sync_queue(void)
 
         spin_lock_irqsave(&sclp_con_lock, flags);
         if (timer_pending(&sclp_con_timer))
-                del_timer_sync(&sclp_con_timer);
+                del_timer(&sclp_con_timer);
         while (sclp_con_queue_running) {
                 spin_unlock_irqrestore(&sclp_con_lock, flags);
                 sclp_sync_wait();
@@ -298,8 +297,8 @@ sclp_console_init(void)
         /* Allocate pages for output buffering */
         INIT_LIST_HEAD(&sclp_con_pages);
         for (i = 0; i < MAX_CONSOLE_PAGES; i++) {
-                page = alloc_bootmem_low_pages(PAGE_SIZE);
-                list_add_tail((struct list_head *) page, &sclp_con_pages);
+                page = (void *) get_zeroed_page(GFP_KERNEL | GFP_DMA);
+                list_add_tail(page, &sclp_con_pages);
         }
         INIT_LIST_HEAD(&sclp_con_outqueue);
         spin_lock_init(&sclp_con_lock);
diff --git a/drivers/s390/char/sclp_vt220.c b/drivers/s390/char/sclp_vt220.c
index 5518e24946aa..178724f2a4c3 100644
--- a/drivers/s390/char/sclp_vt220.c
+++ b/drivers/s390/char/sclp_vt220.c
@@ -20,7 +20,6 @@
 #include <linux/major.h>
 #include <linux/console.h>
 #include <linux/kdev_t.h>
-#include <linux/bootmem.h>
 #include <linux/interrupt.h>
 #include <linux/init.h>
 #include <linux/reboot.h>
@@ -601,10 +600,7 @@ static void __init __sclp_vt220_free_pages(void)
 
         list_for_each_safe(page, p, &sclp_vt220_empty) {
                 list_del(page);
-                if (slab_is_available())
-                        free_page((unsigned long) page);
-                else
-                        free_bootmem((unsigned long) page, PAGE_SIZE);
+                free_page((unsigned long) page);
         }
 }
 
@@ -640,16 +636,12 @@ static int __init __sclp_vt220_init(int num_pages)
         sclp_vt220_flush_later = 0;
 
         /* Allocate pages for output buffering */
+        rc = -ENOMEM;
         for (i = 0; i < num_pages; i++) {
-                if (slab_is_available())
-                        page = (void *) get_zeroed_page(GFP_KERNEL | GFP_DMA);
-                else
-                        page = alloc_bootmem_low_pages(PAGE_SIZE);
-                if (!page) {
-                        rc = -ENOMEM;
+                page = (void *) get_zeroed_page(GFP_KERNEL | GFP_DMA);
+                if (!page)
                         goto out;
-                }
-                list_add_tail((struct list_head *) page, &sclp_vt220_empty);
+                list_add_tail(page, &sclp_vt220_empty);
         }
         rc = sclp_register(&sclp_vt220_register);
 out:
diff --git a/drivers/s390/char/tape_core.c b/drivers/s390/char/tape_core.c
index 595aa04cfd01..1d420d947596 100644
--- a/drivers/s390/char/tape_core.c
+++ b/drivers/s390/char/tape_core.c
@@ -396,7 +396,7 @@ int tape_generic_pm_suspend(struct ccw_device *cdev)
 {
         struct tape_device *device;
 
-        device = cdev->dev.driver_data;
+        device = dev_get_drvdata(&cdev->dev);
         if (!device) {
                 return -ENODEV;
         }
diff --git a/drivers/s390/char/vmlogrdr.c b/drivers/s390/char/vmlogrdr.c
index 411cfa3c7719..c20a4fe6da51 100644
--- a/drivers/s390/char/vmlogrdr.c
+++ b/drivers/s390/char/vmlogrdr.c
@@ -663,7 +663,7 @@ static struct attribute *vmlogrdr_attrs[] = {
 static int vmlogrdr_pm_prepare(struct device *dev)
 {
         int rc;
-        struct vmlogrdr_priv_t *priv = dev->driver_data;
+        struct vmlogrdr_priv_t *priv = dev_get_drvdata(dev);
 
         rc = 0;
         if (priv) {
@@ -753,7 +753,7 @@ static int vmlogrdr_register_device(struct vmlogrdr_priv_t *priv)
                 dev->bus = &iucv_bus;
                 dev->parent = iucv_root;
                 dev->driver = &vmlogrdr_driver;
-                dev->driver_data = priv;
+                dev_set_drvdata(dev, priv);
                 /*
                  * The release function could be called after the
                  * module has been unloaded. It's _only_ task is to
diff --git a/drivers/s390/char/vmur.c b/drivers/s390/char/vmur.c
index 7d9e67cb6471..31b902e94f7b 100644
--- a/drivers/s390/char/vmur.c
+++ b/drivers/s390/char/vmur.c
@@ -170,7 +170,7 @@ static void urdev_put(struct urdev *urd)
  */
 static int ur_pm_suspend(struct ccw_device *cdev)
 {
-        struct urdev *urd = cdev->dev.driver_data;
+        struct urdev *urd = dev_get_drvdata(&cdev->dev);
 
         TRACE("ur_pm_suspend: cdev=%p\n", cdev);
         if (urd->open_flag) {
diff --git a/drivers/s390/cio/qdio.h b/drivers/s390/cio/qdio.h
index 13bcb8114388..b1241f8fae88 100644
--- a/drivers/s390/cio/qdio.h
+++ b/drivers/s390/cio/qdio.h
@@ -351,15 +351,6 @@ static inline unsigned long long get_usecs(void)
         ((bufnr - dec) & QDIO_MAX_BUFFERS_MASK)
 
 /* prototypes for thin interrupt */
-void qdio_sync_after_thinint(struct qdio_q *q);
-int get_buf_state(struct qdio_q *q, unsigned int bufnr, unsigned char *state,
-                  int auto_ack);
-void qdio_check_outbound_after_thinint(struct qdio_q *q);
-int qdio_inbound_q_moved(struct qdio_q *q);
-void qdio_kick_handler(struct qdio_q *q);
-void qdio_stop_polling(struct qdio_q *q);
-int qdio_siga_sync_q(struct qdio_q *q);
-
 void qdio_setup_thinint(struct qdio_irq *irq_ptr);
 int qdio_establish_thinint(struct qdio_irq *irq_ptr);
 void qdio_shutdown_thinint(struct qdio_irq *irq_ptr);
@@ -392,4 +383,6 @@ void qdio_setup_destroy_sysfs(struct ccw_device *cdev);
 int qdio_setup_init(void);
 void qdio_setup_exit(void);
 
+int debug_get_buf_state(struct qdio_q *q, unsigned int bufnr,
+                        unsigned char *state);
 #endif /* _CIO_QDIO_H */
diff --git a/drivers/s390/cio/qdio_debug.c b/drivers/s390/cio/qdio_debug.c
index e3434b34f86c..b8626d4df116 100644
--- a/drivers/s390/cio/qdio_debug.c
+++ b/drivers/s390/cio/qdio_debug.c
@@ -70,9 +70,8 @@ static int qstat_show(struct seq_file *m, void *v)
         seq_printf(m, "slsb buffer states:\n");
         seq_printf(m, "|0      |8      |16     |24     |32     |40     |48     |56  63|\n");
 
-        qdio_siga_sync_q(q);
         for (i = 0; i < QDIO_MAX_BUFFERS_PER_Q; i++) {
-                get_buf_state(q, i, &state, 0);
+                debug_get_buf_state(q, i, &state);
                 switch (state) {
                 case SLSB_P_INPUT_NOT_INIT:
                 case SLSB_P_OUTPUT_NOT_INIT:
diff --git a/drivers/s390/cio/qdio_main.c b/drivers/s390/cio/qdio_main.c
index d79cf5bf0e62..0038750ad945 100644
--- a/drivers/s390/cio/qdio_main.c
+++ b/drivers/s390/cio/qdio_main.c
@@ -231,8 +231,8 @@ static inline int get_buf_states(struct qdio_q *q, unsigned int bufnr,
         return i;
 }
 
-inline int get_buf_state(struct qdio_q *q, unsigned int bufnr,
-                  unsigned char *state, int auto_ack)
+static inline int get_buf_state(struct qdio_q *q, unsigned int bufnr,
+                                unsigned char *state, int auto_ack)
 {
         return get_buf_states(q, bufnr, state, 1, auto_ack);
 }
@@ -276,7 +276,7 @@ void qdio_init_buf_states(struct qdio_irq *irq_ptr)
                                QDIO_MAX_BUFFERS_PER_Q);
 }
 
-static int qdio_siga_sync(struct qdio_q *q, unsigned int output,
+static inline int qdio_siga_sync(struct qdio_q *q, unsigned int output,
                           unsigned int input)
 {
         int cc;
@@ -293,7 +293,7 @@ static int qdio_siga_sync(struct qdio_q *q, unsigned int output,
         return cc;
 }
 
-inline int qdio_siga_sync_q(struct qdio_q *q)
+static inline int qdio_siga_sync_q(struct qdio_q *q)
 {
         if (q->is_input_q)
                 return qdio_siga_sync(q, 0, q->mask);
@@ -358,8 +358,7 @@ static inline int qdio_siga_input(struct qdio_q *q)
         return cc;
 }
 
-/* called from thinint inbound handler */
-void qdio_sync_after_thinint(struct qdio_q *q)
+static inline void qdio_sync_after_thinint(struct qdio_q *q)
 {
         if (pci_out_supported(q)) {
                 if (need_siga_sync_thinint(q))
@@ -370,7 +369,14 @@ void qdio_sync_after_thinint(struct qdio_q *q)
                 qdio_siga_sync_q(q);
 }
 
-inline void qdio_stop_polling(struct qdio_q *q)
+int debug_get_buf_state(struct qdio_q *q, unsigned int bufnr,
+                        unsigned char *state)
+{
+        qdio_siga_sync_q(q);
+        return get_buf_states(q, bufnr, state, 1, 0);
+}
+
+static inline void qdio_stop_polling(struct qdio_q *q)
 {
         if (!q->u.in.polling)
                 return;
@@ -449,13 +455,6 @@ static inline void inbound_primed(struct qdio_q *q, int count)
         count--;
         if (!count)
                 return;
-
-        /*
-         * Need to change all PRIMED buffers to NOT_INIT, otherwise
-         * we're loosing initiative in the thinint code.
-         */
-        set_buf_states(q, q->first_to_check, SLSB_P_INPUT_NOT_INIT,
-                       count);
 }
 
 static int get_inbound_buffer_frontier(struct qdio_q *q)
@@ -470,19 +469,13 @@ static int get_inbound_buffer_frontier(struct qdio_q *q)
         count = min(atomic_read(&q->nr_buf_used), QDIO_MAX_BUFFERS_MASK);
         stop = add_buf(q->first_to_check, count);
 
-        /*
-         * No siga sync here, as a PCI or we after a thin interrupt
-         * will sync the queues.
-         */
-
-        /* need to set count to 1 for non-qebsm */
-        if (!is_qebsm(q))
-                count = 1;
-
-check_next:
         if (q->first_to_check == stop)
                 goto out;
 
+        /*
+         * No siga sync here, as a PCI or we after a thin interrupt
+         * already sync'ed the queues.
+         */
         count = get_buf_states(q, q->first_to_check, &state, count, 1);
         if (!count)
                 goto out;
@@ -490,14 +483,9 @@ check_next:
         switch (state) {
         case SLSB_P_INPUT_PRIMED:
                 inbound_primed(q, count);
-                /*
-                 * No siga-sync needed for non-qebsm here, as the inbound queue
-                 * will be synced on the next siga-r, resp.
-                 * tiqdio_is_inbound_q_done will do the siga-sync.
-                 */
                 q->first_to_check = add_buf(q->first_to_check, count);
                 atomic_sub(count, &q->nr_buf_used);
-                goto check_next;
+                break;
         case SLSB_P_INPUT_ERROR:
                 announce_buffer_error(q, count);
                 /* process the buffer, the upper layer will take care of it */
@@ -516,7 +504,7 @@ out:
         return q->first_to_check;
 }
 
-int qdio_inbound_q_moved(struct qdio_q *q)
+static int qdio_inbound_q_moved(struct qdio_q *q)
 {
         int bufnr;
 
@@ -524,35 +512,32 @@ int qdio_inbound_q_moved(struct qdio_q *q)
 
         if ((bufnr != q->last_move) || q->qdio_error) {
                 q->last_move = bufnr;
-                if (!need_siga_sync(q) && !pci_out_supported(q))
+                if (!is_thinint_irq(q->irq_ptr) && !MACHINE_IS_VM)
                         q->u.in.timestamp = get_usecs();
-
-                DBF_DEV_EVENT(DBF_INFO, q->irq_ptr, "in moved");
                 return 1;
         } else
                 return 0;
 }
 
-static int qdio_inbound_q_done(struct qdio_q *q)
+static inline int qdio_inbound_q_done(struct qdio_q *q)
 {
         unsigned char state = 0;
 
         if (!atomic_read(&q->nr_buf_used))
                 return 1;
 
-        /*
-         * We need that one for synchronization with the adapter, as it
-         * does a kind of PCI avoidance.
-         */
         qdio_siga_sync_q(q);
-
         get_buf_state(q, q->first_to_check, &state, 0);
+
         if (state == SLSB_P_INPUT_PRIMED)
-                /* we got something to do */
+                /* more work coming */
                 return 0;
 
-        /* on VM, we don't poll, so the q is always done here */
-        if (need_siga_sync(q) || pci_out_supported(q))
+        if (is_thinint_irq(q->irq_ptr))
+                return 1;
+
+        /* don't poll under z/VM */
+        if (MACHINE_IS_VM)
                 return 1;
 
         /*
@@ -563,14 +548,11 @@ static int qdio_inbound_q_done(struct qdio_q *q)
                 DBF_DEV_EVENT(DBF_INFO, q->irq_ptr, "in done:%3d",
                               q->first_to_check);
                 return 1;
-        } else {
-                DBF_DEV_EVENT(DBF_INFO, q->irq_ptr, "in notd:%3d",
-                              q->first_to_check);
+        } else
                 return 0;
-        }
 }
 
-void qdio_kick_handler(struct qdio_q *q)
+static void qdio_kick_handler(struct qdio_q *q)
 {
         int start = q->first_to_kick;
         int end = q->first_to_check;
@@ -619,7 +601,6 @@ again:
                 goto again;
 }
 
-/* inbound tasklet */
 void qdio_inbound_processing(unsigned long data)
 {
         struct qdio_q *q = (struct qdio_q *)data;
@@ -642,11 +623,6 @@ static int get_outbound_buffer_frontier(struct qdio_q *q)
         count = min(atomic_read(&q->nr_buf_used), QDIO_MAX_BUFFERS_MASK);
         stop = add_buf(q->first_to_check, count);
 
-        /* need to set count to 1 for non-qebsm */
-        if (!is_qebsm(q))
-                count = 1;
-
-check_next:
         if (q->first_to_check == stop)
                 return q->first_to_check;
 
@@ -661,13 +637,7 @@ check_next:
 
                 atomic_sub(count, &q->nr_buf_used);
                 q->first_to_check = add_buf(q->first_to_check, count);
-                /*
-                 * We fetch all buffer states at once. get_buf_states may
-                 * return count < stop. For QEBSM we do not loop.
-                 */
-                if (is_qebsm(q))
-                        break;
-                goto check_next;
+                break;
         case SLSB_P_OUTPUT_ERROR:
                 announce_buffer_error(q, count);
                 /* process the buffer, the upper layer will take care of it */
@@ -797,8 +767,7 @@ void qdio_outbound_timer(unsigned long data)
         tasklet_schedule(&q->tasklet);
 }
 
-/* called from thinint inbound tasklet */
-void qdio_check_outbound_after_thinint(struct qdio_q *q)
+static inline void qdio_check_outbound_after_thinint(struct qdio_q *q)
 {
         struct qdio_q *out;
         int i;
@@ -811,6 +780,46 @@ void qdio_check_outbound_after_thinint(struct qdio_q *q)
                         tasklet_schedule(&out->tasklet);
 }
 
+static void __tiqdio_inbound_processing(struct qdio_q *q)
+{
+        qdio_perf_stat_inc(&perf_stats.thinint_inbound);
+        qdio_sync_after_thinint(q);
+
+        /*
+         * The interrupt could be caused by a PCI request. Check the
+         * PCI capable outbound queues.
+         */
+        qdio_check_outbound_after_thinint(q);
+
+        if (!qdio_inbound_q_moved(q))
+                return;
+
+        qdio_kick_handler(q);
+
+        if (!qdio_inbound_q_done(q)) {
+                qdio_perf_stat_inc(&perf_stats.thinint_inbound_loop);
+                if (likely(q->irq_ptr->state != QDIO_IRQ_STATE_STOPPED))
+                        tasklet_schedule(&q->tasklet);
+        }
+
+        qdio_stop_polling(q);
+        /*
+         * We need to check again to not lose initiative after
+         * resetting the ACK state.
+         */
+        if (!qdio_inbound_q_done(q)) {
+                qdio_perf_stat_inc(&perf_stats.thinint_inbound_loop2);
+                if (likely(q->irq_ptr->state != QDIO_IRQ_STATE_STOPPED))
+                        tasklet_schedule(&q->tasklet);
+        }
+}
+
+void tiqdio_inbound_processing(unsigned long data)
+{
+        struct qdio_q *q = (struct qdio_q *)data;
+        __tiqdio_inbound_processing(q);
+}
+
 static inline void qdio_set_state(struct qdio_irq *irq_ptr,
                                   enum qdio_irq_states state)
 {
@@ -1488,18 +1497,13 @@ out:
  * @count: how many buffers to process
  */
 int do_QDIO(struct ccw_device *cdev, unsigned int callflags,
-            int q_nr, int bufnr, int count)
+            int q_nr, unsigned int bufnr, unsigned int count)
 {
         struct qdio_irq *irq_ptr;
 
-        if ((bufnr > QDIO_MAX_BUFFERS_PER_Q) ||
-            (count > QDIO_MAX_BUFFERS_PER_Q) ||
-            (q_nr >= QDIO_MAX_QUEUES_PER_IRQ))
+        if (bufnr >= QDIO_MAX_BUFFERS_PER_Q || count > QDIO_MAX_BUFFERS_PER_Q)
                 return -EINVAL;
 
-        if (!count)
-                return 0;
-
         irq_ptr = cdev->private->qdio_data;
         if (!irq_ptr)
                 return -ENODEV;
diff --git a/drivers/s390/cio/qdio_thinint.c b/drivers/s390/cio/qdio_thinint.c
index c655d011a78d..981a77ea7ee2 100644
--- a/drivers/s390/cio/qdio_thinint.c
+++ b/drivers/s390/cio/qdio_thinint.c
@@ -43,9 +43,6 @@ struct indicator_t {
 };
 static struct indicator_t *q_indicators;
 
-static void tiqdio_tasklet_fn(unsigned long data);
-static DECLARE_TASKLET(tiqdio_tasklet, tiqdio_tasklet_fn, 0);
-
 static int css_qdio_omit_svs;
 
 static inline unsigned long do_clear_global_summary(void)
@@ -103,11 +100,6 @@ void tiqdio_add_input_queues(struct qdio_irq *irq_ptr)
         xchg(irq_ptr->dsci, 1);
 }
 
-/*
- * we cannot stop the tiqdio tasklet here since it is for all
- * thinint qdio devices and it must run as long as there is a
- * thinint device left
- */
 void tiqdio_remove_input_queues(struct qdio_irq *irq_ptr)
 {
         struct qdio_q *q;
@@ -126,79 +118,39 @@ void tiqdio_remove_input_queues(struct qdio_irq *irq_ptr)
         }
 }
 
-static inline int tiqdio_inbound_q_done(struct qdio_q *q)
-{
-        unsigned char state = 0;
-
-        if (!atomic_read(&q->nr_buf_used))
-                return 1;
-
-        qdio_siga_sync_q(q);
-        get_buf_state(q, q->first_to_check, &state, 0);
-
-        if (state == SLSB_P_INPUT_PRIMED)
-                /* more work coming */
-                return 0;
-        return 1;
-}
-
 static inline int shared_ind(struct qdio_irq *irq_ptr)
 {
         return irq_ptr->dsci == &q_indicators[TIQDIO_SHARED_IND].ind;
 }
 
-static void __tiqdio_inbound_processing(struct qdio_q *q)
+/**
+ * tiqdio_thinint_handler - thin interrupt handler for qdio
+ * @ind: pointer to adapter local summary indicator
+ * @drv_data: NULL
+ */
+static void tiqdio_thinint_handler(void *ind, void *drv_data)
 {
-        qdio_perf_stat_inc(&perf_stats.thinint_inbound);
-        qdio_sync_after_thinint(q);
+        struct qdio_q *q;
+
+        qdio_perf_stat_inc(&perf_stats.thin_int);
 
         /*
-         * Maybe we have work on our outbound queues... at least
-         * we have to check the PCI capable queues.
+         * SVS only when needed: issue SVS to benefit from iqdio interrupt
+         * avoidance (SVS clears adapter interrupt suppression overwrite)
          */
-        qdio_check_outbound_after_thinint(q);
-
-        if (!qdio_inbound_q_moved(q))
-                return;
-
-        qdio_kick_handler(q);
-
-        if (!tiqdio_inbound_q_done(q)) {
-                qdio_perf_stat_inc(&perf_stats.thinint_inbound_loop);
-                if (likely(q->irq_ptr->state != QDIO_IRQ_STATE_STOPPED))
-                        tasklet_schedule(&q->tasklet);
-        }
+        if (!css_qdio_omit_svs)
+                do_clear_global_summary();
 
-        qdio_stop_polling(q);
         /*
-         * We need to check again to not lose initiative after
-         * resetting the ACK state.
+         * reset local summary indicator (tiqdio_alsi) to stop adapter
+         * interrupts for now
          */
-        if (!tiqdio_inbound_q_done(q)) {
-                qdio_perf_stat_inc(&perf_stats.thinint_inbound_loop2);
-                if (likely(q->irq_ptr->state != QDIO_IRQ_STATE_STOPPED))
-                        tasklet_schedule(&q->tasklet);
-        }
-}
-
-void tiqdio_inbound_processing(unsigned long data)
-{
-        struct qdio_q *q = (struct qdio_q *)data;
-
-        __tiqdio_inbound_processing(q);
-}
-
-/* check for work on all inbound thinint queues */
-static void tiqdio_tasklet_fn(unsigned long data)
-{
-        struct qdio_q *q;
-
-        qdio_perf_stat_inc(&perf_stats.tasklet_thinint);
-again:
+        xchg((u8 *)ind, 0);
 
         /* protect tiq_list entries, only changed in activate or shutdown */
         rcu_read_lock();
 
+        /* check for work on all inbound thinint queues */
         list_for_each_entry_rcu(q, &tiq_list, entry)
                 /* only process queues from changed sets */
                 if (*q->irq_ptr->dsci) {
@@ -226,37 +178,6 @@ again:
                 if (*tiqdio_alsi)
                         xchg(&q_indicators[TIQDIO_SHARED_IND].ind, 1);
         }
-
-        /* check for more work */
-        if (*tiqdio_alsi) {
-                xchg(tiqdio_alsi, 0);
-                qdio_perf_stat_inc(&perf_stats.tasklet_thinint_loop);
-                goto again;
-        }
-}
-
-/**
- * tiqdio_thinint_handler - thin interrupt handler for qdio
- * @ind: pointer to adapter local summary indicator
- * @drv_data: NULL
- */
-static void tiqdio_thinint_handler(void *ind, void *drv_data)
-{
-        qdio_perf_stat_inc(&perf_stats.thin_int);
-
-        /*
-         * SVS only when needed: issue SVS to benefit from iqdio interrupt
-         * avoidance (SVS clears adapter interrupt suppression overwrite)
-         */
-        if (!css_qdio_omit_svs)
-                do_clear_global_summary();
-
-        /*
-         * reset local summary indicator (tiqdio_alsi) to stop adapter
-         * interrupts for now, the tasklet will clean all dsci's
-         */
-        xchg((u8 *)ind, 0);
-        tasklet_hi_schedule(&tiqdio_tasklet);
 }
 
 static int set_subchannel_ind(struct qdio_irq *irq_ptr, int reset)
@@ -376,5 +297,4 @@ void __exit tiqdio_unregister_thinints(void)
                 s390_unregister_adapter_interrupt(tiqdio_alsi, QDIO_AIRQ_ISC);
                 isc_unregister(QDIO_AIRQ_ISC);
         }
-        tasklet_kill(&tiqdio_tasklet);
 }
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index 9c148406b980..727a809636d8 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -54,6 +54,12 @@ static int ap_poll_thread_start(void);
 static void ap_poll_thread_stop(void);
 static void ap_request_timeout(unsigned long);
 static inline void ap_schedule_poll_timer(void);
+static int __ap_poll_device(struct ap_device *ap_dev, unsigned long *flags);
+static int ap_device_remove(struct device *dev);
+static int ap_device_probe(struct device *dev);
+static void ap_interrupt_handler(void *unused1, void *unused2);
+static void ap_reset(struct ap_device *ap_dev);
+static void ap_config_timeout(unsigned long ptr);
 
 /*
  * Module description.
@@ -101,6 +107,10 @@ static struct hrtimer ap_poll_timer;
  * If z/VM change to 1500000 nanoseconds to adjust to z/VM polling.*/
 static unsigned long long poll_timeout = 250000;
 
+/* Suspend flag */
+static int ap_suspend_flag;
+static struct bus_type ap_bus_type;
+
 /**
  * ap_using_interrupts() - Returns non-zero if interrupt support is
  * available.
@@ -617,10 +627,79 @@ static int ap_uevent (struct device *dev, struct kobj_uevent_env *env)
         return retval;
 }
 
+static int ap_bus_suspend(struct device *dev, pm_message_t state)
+{
+        struct ap_device *ap_dev = to_ap_dev(dev);
+        unsigned long flags;
+
+        if (!ap_suspend_flag) {
+                ap_suspend_flag = 1;
+
+                /* Disable scanning for devices, thus we do not want to scan
+                 * for them after removing.
+                 */
+                del_timer_sync(&ap_config_timer);
+                if (ap_work_queue != NULL) {
+                        destroy_workqueue(ap_work_queue);
+                        ap_work_queue = NULL;
+                }
+                tasklet_disable(&ap_tasklet);
+        }
+        /* Poll on the device until all requests are finished. */
+        do {
+                flags = 0;
+                __ap_poll_device(ap_dev, &flags);
+        } while ((flags & 1) || (flags & 2));
+
+        ap_device_remove(dev);
+        return 0;
+}
+
+static int ap_bus_resume(struct device *dev)
+{
+        int rc = 0;
+        struct ap_device *ap_dev = to_ap_dev(dev);
+
+        if (ap_suspend_flag) {
+                ap_suspend_flag = 0;
+                if (!ap_interrupts_available())
+                        ap_interrupt_indicator = NULL;
+                ap_device_probe(dev);
+                ap_reset(ap_dev);
+                setup_timer(&ap_dev->timeout, ap_request_timeout,
+                            (unsigned long) ap_dev);
+                ap_scan_bus(NULL);
+                init_timer(&ap_config_timer);
+                ap_config_timer.function = ap_config_timeout;
+                ap_config_timer.data = 0;
+                ap_config_timer.expires = jiffies + ap_config_time * HZ;
+                add_timer(&ap_config_timer);
+                ap_work_queue = create_singlethread_workqueue("kapwork");
+                if (!ap_work_queue)
+                        return -ENOMEM;
+                tasklet_enable(&ap_tasklet);
+                if (!ap_using_interrupts())
+                        ap_schedule_poll_timer();
+                else
+                        tasklet_schedule(&ap_tasklet);
+                if (ap_thread_flag)
+                        rc = ap_poll_thread_start();
+        } else {
+                ap_device_probe(dev);
+                ap_reset(ap_dev);
+                setup_timer(&ap_dev->timeout, ap_request_timeout,
+                            (unsigned long) ap_dev);
+        }
+
+        return rc;
+}
+
 static struct bus_type ap_bus_type = {
         .name = "ap",
         .match = &ap_bus_match,
         .uevent = &ap_uevent,
+        .suspend = ap_bus_suspend,
+        .resume = ap_bus_resume
 };
 
 static int ap_device_probe(struct device *dev)
@@ -1066,7 +1145,7 @@ ap_config_timeout(unsigned long ptr)
  */
 static inline void ap_schedule_poll_timer(void)
 {
-        if (ap_using_interrupts())
+        if (ap_using_interrupts() || ap_suspend_flag)
                 return;
         if (hrtimer_is_queued(&ap_poll_timer))
                 return;
@@ -1384,6 +1463,8 @@ static int ap_poll_thread(void *data)
 
         set_user_nice(current, 19);
         while (1) {
+                if (ap_suspend_flag)
+                        return 0;
                 if (need_resched()) {
                         schedule();
                         continue;
@@ -1414,7 +1495,7 @@ static int ap_poll_thread_start(void)
 {
         int rc;
 
-        if (ap_using_interrupts())
+        if (ap_using_interrupts() || ap_suspend_flag)
                 return 0;
         mutex_lock(&ap_poll_thread_mutex);
         if (!ap_poll_kthread) {
diff --git a/drivers/s390/net/netiucv.c b/drivers/s390/net/netiucv.c
index 52574ce797b2..8c36eafcfbfe 100644
--- a/drivers/s390/net/netiucv.c
+++ b/drivers/s390/net/netiucv.c
@@ -1307,7 +1307,7 @@ static void netiucv_pm_complete(struct device *dev)
  */
 static int netiucv_pm_freeze(struct device *dev)
 {
-        struct netiucv_priv *priv = dev->driver_data;
+        struct netiucv_priv *priv = dev_get_drvdata(dev);
         struct net_device *ndev = NULL;
         int rc = 0;
 
@@ -1331,7 +1331,7 @@ out:
  */
 static int netiucv_pm_restore_thaw(struct device *dev)
 {
-        struct netiucv_priv *priv = dev->driver_data;
+        struct netiucv_priv *priv = dev_get_drvdata(dev);
         struct net_device *ndev = NULL;
         int rc = 0;
 
diff --git a/drivers/serial/Kconfig b/drivers/serial/Kconfig
index 1132c5cae7ab..037c1e0b7c4c 100644
--- a/drivers/serial/Kconfig
+++ b/drivers/serial/Kconfig
@@ -1320,6 +1320,16 @@ config SERIAL_SGI_IOC3
           If you have an SGI Altix with an IOC3 serial card,
           say Y or M.  Otherwise, say N.
 
+config SERIAL_MSM
+        bool "MSM on-chip serial port support"
+        depends on ARM && ARCH_MSM
+        select SERIAL_CORE
+
+config SERIAL_MSM_CONSOLE
+        bool "MSM serial console support"
+        depends on SERIAL_MSM=y
+        select SERIAL_CORE_CONSOLE
+
 config SERIAL_NETX
         tristate "NetX serial port support"
         depends on ARM && ARCH_NETX
diff --git a/drivers/serial/Makefile b/drivers/serial/Makefile
index 45a8658f54d5..d5a29981c6c4 100644
--- a/drivers/serial/Makefile
+++ b/drivers/serial/Makefile
@@ -71,6 +71,7 @@ obj-$(CONFIG_SERIAL_SGI_IOC4) += ioc4_serial.o
 obj-$(CONFIG_SERIAL_SGI_IOC3) += ioc3_serial.o
 obj-$(CONFIG_SERIAL_ATMEL) += atmel_serial.o
 obj-$(CONFIG_SERIAL_UARTLITE) += uartlite.o
+obj-$(CONFIG_SERIAL_MSM) += msm_serial.o
 obj-$(CONFIG_SERIAL_NETX) += netx-serial.o
 obj-$(CONFIG_SERIAL_OF_PLATFORM) += of_serial.o
 obj-$(CONFIG_SERIAL_OF_PLATFORM_NWPSERIAL) += nwpserial.o
diff --git a/drivers/serial/bfin_5xx.c b/drivers/serial/bfin_5xx.c
index e2f6b1bfac98..b4a7650af696 100644
--- a/drivers/serial/bfin_5xx.c
+++ b/drivers/serial/bfin_5xx.c
@@ -38,6 +38,10 @@
 #include <asm/cacheflush.h>
 #endif
 
+#ifdef CONFIG_SERIAL_BFIN_MODULE
+# undef CONFIG_EARLY_PRINTK
+#endif
+
 /* UART name and device definitions */
 #define BFIN_SERIAL_NAME        "ttyBF"
 #define BFIN_SERIAL_MAJOR       204
@@ -1110,6 +1114,7 @@ static void __init bfin_serial_init_ports(void)
         bfin_serial_hw_init();
 
         for (i = 0; i < nr_active_ports; i++) {
+                spin_lock_init(&bfin_serial_ports[i].port.lock);
                 bfin_serial_ports[i].port.uartclk   = get_sclk();
                 bfin_serial_ports[i].port.fifosize  = BFIN_UART_TX_FIFO_SIZE;
                 bfin_serial_ports[i].port.ops       = &bfin_serial_pops;
diff --git a/drivers/serial/msm_serial.c b/drivers/serial/msm_serial.c
new file mode 100644
index 000000000000..698048f64f5e
--- /dev/null
+++ b/drivers/serial/msm_serial.c
@@ -0,0 +1,772 @@
+/*
+ * drivers/serial/msm_serial.c - driver for msm7k serial device and console
+ *
+ * Copyright (C) 2007 Google, Inc.
+ * Author: Robert Love <rlove@google.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#if defined(CONFIG_SERIAL_MSM_CONSOLE) && defined(CONFIG_MAGIC_SYSRQ)
+# define SUPPORT_SYSRQ
+#endif
+
+#include <linux/hrtimer.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/irq.h>
+#include <linux/init.h>
+#include <linux/console.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+#include <linux/serial_core.h>
+#include <linux/serial.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+
+#include "msm_serial.h"
+
+struct msm_port {
+        struct uart_port        uart;
+        char                    name[16];
+        struct clk              *clk;
+        unsigned int            imr;
+};
+
+#define UART_TO_MSM(uart_port)  ((struct msm_port *) uart_port)
+
+static inline void msm_write(struct uart_port *port, unsigned int val,
+                             unsigned int off)
+{
+        __raw_writel(val, port->membase + off);
+}
+
+static inline unsigned int msm_read(struct uart_port *port, unsigned int off)
+{
+        return __raw_readl(port->membase + off);
+}
+
+static void msm_stop_tx(struct uart_port *port)
+{
+        struct msm_port *msm_port = UART_TO_MSM(port);
+
+        msm_port->imr &= ~UART_IMR_TXLEV;
+        msm_write(port, msm_port->imr, UART_IMR);
+}
+
+static void msm_start_tx(struct uart_port *port)
+{
+        struct msm_port *msm_port = UART_TO_MSM(port);
+
+        msm_port->imr |= UART_IMR_TXLEV;
+        msm_write(port, msm_port->imr, UART_IMR);
+}
+
+static void msm_stop_rx(struct uart_port *port)
+{
+        struct msm_port *msm_port = UART_TO_MSM(port);
+
+        msm_port->imr &= ~(UART_IMR_RXLEV | UART_IMR_RXSTALE);
+        msm_write(port, msm_port->imr, UART_IMR);
+}
+
+static void msm_enable_ms(struct uart_port *port)
+{
+        struct msm_port *msm_port = UART_TO_MSM(port);
+
+        msm_port->imr |= UART_IMR_DELTA_CTS;
+        msm_write(port, msm_port->imr, UART_IMR);
+}
+
+static void handle_rx(struct uart_port *port)
+{
+        struct tty_struct *tty = port->info->port.tty;
+        unsigned int sr;
+
+        /*
+         * Handle overrun. My understanding of the hardware is that overrun
+         * is not tied to the RX buffer, so we handle the case out of band.
+         */
+        if ((msm_read(port, UART_SR) & UART_SR_OVERRUN)) {
+                port->icount.overrun++;
+                tty_insert_flip_char(tty, 0, TTY_OVERRUN);
+                msm_write(port, UART_CR_CMD_RESET_ERR, UART_CR);
+        }
+
+        /* and now the main RX loop */
+        while ((sr = msm_read(port, UART_SR)) & UART_SR_RX_READY) {
+                unsigned int c;
+                char flag = TTY_NORMAL;
+
+                c = msm_read(port, UART_RF);
+
+                if (sr & UART_SR_RX_BREAK) {
+                        port->icount.brk++;
+                        if (uart_handle_break(port))
+                                continue;
+                } else if (sr & UART_SR_PAR_FRAME_ERR) {
+                        port->icount.frame++;
+                } else {
+                        port->icount.rx++;
+                }
+
+                /* Mask conditions we're ignorning. */
+                sr &= port->read_status_mask;
+
+                if (sr & UART_SR_RX_BREAK) {
+                        flag = TTY_BREAK;
+                } else if (sr & UART_SR_PAR_FRAME_ERR) {
+                        flag = TTY_FRAME;
+                }
+
+                if (!uart_handle_sysrq_char(port, c))
+                        tty_insert_flip_char(tty, c, flag);
+        }
+
+        tty_flip_buffer_push(tty);
+}
+
+static void handle_tx(struct uart_port *port)
+{
+        struct circ_buf *xmit = &port->info->xmit;
+        struct msm_port *msm_port = UART_TO_MSM(port);
+        int sent_tx;
+
+        if (port->x_char) {
+                msm_write(port, port->x_char, UART_TF);
+                port->icount.tx++;
+                port->x_char = 0;
+        }
+
+        while (msm_read(port, UART_SR) & UART_SR_TX_READY) {
+                if (uart_circ_empty(xmit)) {
+                        /* disable tx interrupts */
+                        msm_port->imr &= ~UART_IMR_TXLEV;
+                        msm_write(port, msm_port->imr, UART_IMR);
+                        break;
+                }
+
+                msm_write(port, xmit->buf[xmit->tail], UART_TF);
+
+                xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE - 1);
+                port->icount.tx++;
+                sent_tx = 1;
+        }
+
+        if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+                uart_write_wakeup(port);
+}
+
+static void handle_delta_cts(struct uart_port *port)
+{
+        msm_write(port, UART_CR_CMD_RESET_CTS, UART_CR);
+        port->icount.cts++;
+        wake_up_interruptible(&port->info->delta_msr_wait);
+}
+
+static irqreturn_t msm_irq(int irq, void *dev_id)
+{
+        struct uart_port *port = dev_id;
+        struct msm_port *msm_port = UART_TO_MSM(port);
+        unsigned int misr;
+
+        spin_lock(&port->lock);
+        misr = msm_read(port, UART_MISR);
+        msm_write(port, 0, UART_IMR); /* disable interrupt */
+
+        if (misr & (UART_IMR_RXLEV | UART_IMR_RXSTALE))
+                handle_rx(port);
+        if (misr & UART_IMR_TXLEV)
+                handle_tx(port);
+        if (misr & UART_IMR_DELTA_CTS)
+                handle_delta_cts(port);
+
+        msm_write(port, msm_port->imr, UART_IMR); /* restore interrupt */
+        spin_unlock(&port->lock);
+
+        return IRQ_HANDLED;
+}
+
+static unsigned int msm_tx_empty(struct uart_port *port)
+{
+        return (msm_read(port, UART_SR) & UART_SR_TX_EMPTY) ? TIOCSER_TEMT : 0;
+}
+
+static unsigned int msm_get_mctrl(struct uart_port *port)
+{
+        return TIOCM_CAR | TIOCM_CTS | TIOCM_DSR | TIOCM_RTS;
+}
+
+static void msm_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+        unsigned int mr;
+
+        mr = msm_read(port, UART_MR1);
+
+        if (!(mctrl & TIOCM_RTS)) {
+                mr &= ~UART_MR1_RX_RDY_CTL;
+                msm_write(port, mr, UART_MR1);
+                msm_write(port, UART_CR_CMD_RESET_RFR, UART_CR);
+        } else {
+                mr |= UART_MR1_RX_RDY_CTL;
+                msm_write(port, mr, UART_MR1);
+        }
+}
+
+static void msm_break_ctl(struct uart_port *port, int break_ctl)
+{
+        if (break_ctl)
+                msm_write(port, UART_CR_CMD_START_BREAK, UART_CR);
+        else
+                msm_write(port, UART_CR_CMD_STOP_BREAK, UART_CR);
+}
+
+static int msm_set_baud_rate(struct uart_port *port, unsigned int baud)
+{
+        unsigned int baud_code, rxstale, watermark;
+
+        switch (baud) {
+        case 300:
+                baud_code = UART_CSR_300;
+                rxstale = 1;
+                break;
+        case 600:
+                baud_code = UART_CSR_600;
+                rxstale = 1;
+                break;
+        case 1200:
+                baud_code = UART_CSR_1200;
+                rxstale = 1;
+                break;
+        case 2400:
+                baud_code = UART_CSR_2400;
+                rxstale = 1;
+                break;
+        case 4800:
+                baud_code = UART_CSR_4800;
+                rxstale = 1;
+                break;
+        case 9600:
+                baud_code = UART_CSR_9600;
+                rxstale = 2;
+                break;
+        case 14400:
+                baud_code = UART_CSR_14400;
+                rxstale = 3;
+                break;
+        case 19200:
+                baud_code = UART_CSR_19200;
+                rxstale = 4;
+                break;
+        case 28800:
+                baud_code = UART_CSR_28800;
+                rxstale = 6;
+                break;
+        case 38400:
+                baud_code = UART_CSR_38400;
+                rxstale = 8;
+                break;
+        case 57600:
+                baud_code = UART_CSR_57600;
+                rxstale = 16;
+                break;
+        case 115200:
+        default:
+                baud_code = UART_CSR_115200;
+                baud = 115200;
+                rxstale = 31;
+                break;
+        }
+
+        msm_write(port, baud_code, UART_CSR);
+
+        /* RX stale watermark */
+        watermark = UART_IPR_STALE_LSB & rxstale;
+        watermark |= UART_IPR_RXSTALE_LAST;
+        watermark |= UART_IPR_STALE_TIMEOUT_MSB & (rxstale << 2);
+        msm_write(port, watermark, UART_IPR);
+
+        /* set RX watermark */
+        watermark = (port->fifosize * 3) / 4;
+        msm_write(port, watermark, UART_RFWR);
+
+        /* set TX watermark */
+        msm_write(port, 10, UART_TFWR);
+
+        return baud;
+}
+
+static void msm_reset(struct uart_port *port)
+{
+        /* reset everything */
+        msm_write(port, UART_CR_CMD_RESET_RX, UART_CR);
+        msm_write(port, UART_CR_CMD_RESET_TX, UART_CR);
+        msm_write(port, UART_CR_CMD_RESET_ERR, UART_CR);
+        msm_write(port, UART_CR_CMD_RESET_BREAK_INT, UART_CR);
+        msm_write(port, UART_CR_CMD_RESET_CTS, UART_CR);
+        msm_write(port, UART_CR_CMD_SET_RFR, UART_CR);
+}
+
+static void msm_init_clock(struct uart_port *port)
+{
+        struct msm_port *msm_port = UART_TO_MSM(port);
+
+        clk_enable(msm_port->clk);
+
+        msm_write(port, 0xC0, UART_MREG);
+        msm_write(port, 0xB2, UART_NREG);
+        msm_write(port, 0x7D, UART_DREG);
+        msm_write(port, 0x1C, UART_MNDREG);
+}
+
+static int msm_startup(struct uart_port *port)
+{
+        struct msm_port *msm_port = UART_TO_MSM(port);
+        unsigned int data, rfr_level;
+        int ret;
+
+        snprintf(msm_port->name, sizeof(msm_port->name),
+                 "msm_serial%d", port->line);
+
+        ret = request_irq(port->irq, msm_irq, IRQF_TRIGGER_HIGH,
+                          msm_port->name, port);
+        if (unlikely(ret))
+                return ret;
+
+        msm_init_clock(port);
+
+        if (likely(port->fifosize > 12))
+                rfr_level = port->fifosize - 12;
+        else
+                rfr_level = port->fifosize;
+
+        /* set automatic RFR level */
+        data = msm_read(port, UART_MR1);
+        data &= ~UART_MR1_AUTO_RFR_LEVEL1;
+        data &= ~UART_MR1_AUTO_RFR_LEVEL0;
+        data |= UART_MR1_AUTO_RFR_LEVEL1 & (rfr_level << 2);
+        data |= UART_MR1_AUTO_RFR_LEVEL0 & rfr_level;
+        msm_write(port, data, UART_MR1);
+
+        /* make sure that RXSTALE count is non-zero */
+        data = msm_read(port, UART_IPR);
+        if (unlikely(!data)) {
+                data |= UART_IPR_RXSTALE_LAST;
+                data |= UART_IPR_STALE_LSB;
+                msm_write(port, data, UART_IPR);
+        }
+
+        msm_reset(port);
+
+        msm_write(port, 0x05, UART_CR); /* enable TX & RX */
+
+        /* turn on RX and CTS interrupts */
+        msm_port->imr = UART_IMR_RXLEV | UART_IMR_RXSTALE |
+                        UART_IMR_CURRENT_CTS;
+        msm_write(port, msm_port->imr, UART_IMR);
+
+        return 0;
+}
+
+static void msm_shutdown(struct uart_port *port)
+{
+        struct msm_port *msm_port = UART_TO_MSM(port);
+
+        msm_port->imr = 0;
+        msm_write(port, 0, UART_IMR); /* disable interrupts */
+
+        clk_disable(msm_port->clk);
+
+        free_irq(port->irq, port);
+}
+
+static void msm_set_termios(struct uart_port *port, struct ktermios *termios,
+                            struct ktermios *old)
+{
+        unsigned long flags;
+        unsigned int baud, mr;
+
+        spin_lock_irqsave(&port->lock, flags);
+
+        /* calculate and set baud rate */
+        baud = uart_get_baud_rate(port, termios, old, 300, 115200);
+        baud = msm_set_baud_rate(port, baud);
+        if (tty_termios_baud_rate(termios))
+                tty_termios_encode_baud_rate(termios, baud, baud);
+        
+        /* calculate parity */
+        mr = msm_read(port, UART_MR2);
+        mr &= ~UART_MR2_PARITY_MODE;
+        if (termios->c_cflag & PARENB) {
+                if (termios->c_cflag & PARODD)
+                        mr |= UART_MR2_PARITY_MODE_ODD;
+                else if (termios->c_cflag & CMSPAR)
+                        mr |= UART_MR2_PARITY_MODE_SPACE;
+                else
+                        mr |= UART_MR2_PARITY_MODE_EVEN;
+        }
+
+        /* calculate bits per char */
+        mr &= ~UART_MR2_BITS_PER_CHAR;
+        switch (termios->c_cflag & CSIZE) {
+        case CS5:
+                mr |= UART_MR2_BITS_PER_CHAR_5;
+                break;
+        case CS6:
+                mr |= UART_MR2_BITS_PER_CHAR_6;
+                break;
+        case CS7:
+                mr |= UART_MR2_BITS_PER_CHAR_7;
+                break;
+        case CS8:
+        default:
+                mr |= UART_MR2_BITS_PER_CHAR_8;
+                break;
+        }
+
+        /* calculate stop bits */
+        mr &= ~(UART_MR2_STOP_BIT_LEN_ONE | UART_MR2_STOP_BIT_LEN_TWO);
+        if (termios->c_cflag & CSTOPB)
+                mr |= UART_MR2_STOP_BIT_LEN_TWO;
+        else
+                mr |= UART_MR2_STOP_BIT_LEN_ONE;
+
+        /* set parity, bits per char, and stop bit */
+        msm_write(port, mr, UART_MR2);
+
+        /* calculate and set hardware flow control */
+        mr = msm_read(port, UART_MR1);
+        mr &= ~(UART_MR1_CTS_CTL | UART_MR1_RX_RDY_CTL);
+        if (termios->c_cflag & CRTSCTS) {
+                mr |= UART_MR1_CTS_CTL;
+                mr |= UART_MR1_RX_RDY_CTL;
+        }
+        msm_write(port, mr, UART_MR1);
+
+        /* Configure status bits to ignore based on termio flags. */
+        port->read_status_mask = 0;
+        if (termios->c_iflag & INPCK)
+                port->read_status_mask |= UART_SR_PAR_FRAME_ERR;
+        if (termios->c_iflag & (BRKINT | PARMRK))
+                port->read_status_mask |= UART_SR_RX_BREAK;
+
+        uart_update_timeout(port, termios->c_cflag, baud);
+
+        spin_unlock_irqrestore(&port->lock, flags);
+}
+
+static const char *msm_type(struct uart_port *port)
+{
+        return "MSM";
+}
+
+static void msm_release_port(struct uart_port *port)
+{
+        struct platform_device *pdev = to_platform_device(port->dev);
+        struct resource *resource;
+        resource_size_t size;
+
+        resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (unlikely(!resource))
+                return;
+        size = resource->end - resource->start + 1;
+
+        release_mem_region(port->mapbase, size);
+        iounmap(port->membase);
+        port->membase = NULL;
+}
+
+static int msm_request_port(struct uart_port *port)
+{
+        struct platform_device *pdev = to_platform_device(port->dev);
+        struct resource *resource;
+        resource_size_t size;
+
+        resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (unlikely(!resource))
+                return -ENXIO;
+        size = resource->end - resource->start + 1;
+
+        if (unlikely(!request_mem_region(port->mapbase, size, "msm_serial")))
+                return -EBUSY;
+
+        port->membase = ioremap(port->mapbase, size);
+        if (!port->membase) {
+                release_mem_region(port->mapbase, size);
+                return -EBUSY;
+        }
+
+        return 0;
+}
+
+static void msm_config_port(struct uart_port *port, int flags)
+{
+        if (flags & UART_CONFIG_TYPE) {
+                port->type = PORT_MSM;
+                msm_request_port(port);
+        }
+}
+
+static int msm_verify_port(struct uart_port *port, struct serial_struct *ser)
+{
+        if (unlikely(ser->type != PORT_UNKNOWN && ser->type != PORT_MSM))
+                return -EINVAL;
+        if (unlikely(port->irq != ser->irq))
+                return -EINVAL;
+        return 0;
+}
+
+static void msm_power(struct uart_port *port, unsigned int state,
+                      unsigned int oldstate)
+{
+        struct msm_port *msm_port = UART_TO_MSM(port);
+
+        switch (state) {
+        case 0:
+                clk_enable(msm_port->clk);
+                break;
+        case 3:
+                clk_disable(msm_port->clk);
+                break;
+        default:
+                printk(KERN_ERR "msm_serial: Unknown PM state %d\n", state);
+        }
+}
+
+static struct uart_ops msm_uart_pops = {
+        .tx_empty = msm_tx_empty,
+        .set_mctrl = msm_set_mctrl,
+        .get_mctrl = msm_get_mctrl,
+        .stop_tx = msm_stop_tx,
+        .start_tx = msm_start_tx,
+        .stop_rx = msm_stop_rx,
+        .enable_ms = msm_enable_ms,
+        .break_ctl = msm_break_ctl,
+        .startup = msm_startup,
+        .shutdown = msm_shutdown,
+        .set_termios = msm_set_termios,
+        .type = msm_type,
+        .release_port = msm_release_port,
+        .request_port = msm_request_port,
+        .config_port = msm_config_port,
+        .verify_port = msm_verify_port,
+        .pm = msm_power,
+};
+
+static struct msm_port msm_uart_ports[] = {
+        {
+                .uart = {
+                        .iotype = UPIO_MEM,
+                        .ops = &msm_uart_pops,
+                        .flags = UPF_BOOT_AUTOCONF,
+                        .fifosize = 512,
+                        .line = 0,
+                },
+        },
+        {
+                .uart = {
+                        .iotype = UPIO_MEM,
+                        .ops = &msm_uart_pops,
+                        .flags = UPF_BOOT_AUTOCONF,
+                        .fifosize = 512,
+                        .line = 1,
+                },
+        },
+        {
+                .uart = {
+                        .iotype = UPIO_MEM,
+                        .ops = &msm_uart_pops,
+                        .flags = UPF_BOOT_AUTOCONF,
+                        .fifosize = 64,
+                        .line = 2,
+                },
+        },
+};
+
+#define UART_NR ARRAY_SIZE(msm_uart_ports)
+
+static inline struct uart_port *get_port_from_line(unsigned int line)
+{
+        return &msm_uart_ports[line].uart;
+}
+
+#ifdef CONFIG_SERIAL_MSM_CONSOLE
+
+static void msm_console_putchar(struct uart_port *port, int c)
+{
+        while (!(msm_read(port, UART_SR) & UART_SR_TX_READY))
+                ;
+        msm_write(port, c, UART_TF);
+}
+
+static void msm_console_write(struct console *co, const char *s,
+                              unsigned int count)
+{
+        struct uart_port *port;
+        struct msm_port *msm_port;
+
+        BUG_ON(co->index < 0 || co->index >= UART_NR);
+
+        port = get_port_from_line(co->index);
+        msm_port = UART_TO_MSM(port);
+
+        spin_lock(&port->lock);
+        uart_console_write(port, s, count, msm_console_putchar);
+        spin_unlock(&port->lock);
+}
+
+static int __init msm_console_setup(struct console *co, char *options)
+{
+        struct uart_port *port;
+        int baud, flow, bits, parity;
+
+        if (unlikely(co->index >= UART_NR || co->index < 0))
+                return -ENXIO;
+
+        port = get_port_from_line(co->index);
+
+        if (unlikely(!port->membase))
+                return -ENXIO;
+
+        port->cons = co;
+
+        msm_init_clock(port);
+
+        if (options)
+                uart_parse_options(options, &baud, &parity, &bits, &flow);
+
+        bits = 8;
+        parity = 'n';
+        flow = 'n';
+        msm_write(port, UART_MR2_BITS_PER_CHAR_8 | UART_MR2_STOP_BIT_LEN_ONE,
+                  UART_MR2);    /* 8N1 */
+
+        if (baud < 300 || baud > 115200)
+                baud = 115200;
+        msm_set_baud_rate(port, baud);
+
+        msm_reset(port);
+
+        printk(KERN_INFO "msm_serial: console setup on port #%d\n", port->line);
+
+        return uart_set_options(port, co, baud, parity, bits, flow);
+}
+
+static struct uart_driver msm_uart_driver;
+
+static struct console msm_console = {
+        .name = "ttyMSM",
+        .write = msm_console_write,
+        .device = uart_console_device,
+        .setup = msm_console_setup,
+        .flags = CON_PRINTBUFFER,
+        .index = -1,
+        .data = &msm_uart_driver,
+};
+
+#define MSM_CONSOLE     (&msm_console)
+
+#else
+#define MSM_CONSOLE     NULL
+#endif
+
+static struct uart_driver msm_uart_driver = {
+        .owner = THIS_MODULE,
+        .driver_name = "msm_serial",
+        .dev_name = "ttyMSM",
+        .nr = UART_NR,
+        .cons = MSM_CONSOLE,
+};
+
+static int __init msm_serial_probe(struct platform_device *pdev)
+{
+        struct msm_port *msm_port;
+        struct resource *resource;
+        struct uart_port *port;
+
+        if (unlikely(pdev->id < 0 || pdev->id >= UART_NR))
+                return -ENXIO;
+
+        printk(KERN_INFO "msm_serial: detected port #%d\n", pdev->id);
+
+        port = get_port_from_line(pdev->id);
+        port->dev = &pdev->dev;
+        msm_port = UART_TO_MSM(port);
+
+        msm_port->clk = clk_get(&pdev->dev, "uart_clk");
+        if (unlikely(IS_ERR(msm_port->clk)))
+                return PTR_ERR(msm_port->clk);
+        port->uartclk = clk_get_rate(msm_port->clk);
+
+        resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (unlikely(!resource))
+                return -ENXIO;
+        port->mapbase = resource->start;
+
+        port->irq = platform_get_irq(pdev, 0);
+        if (unlikely(port->irq < 0))
+                return -ENXIO;
+
+        platform_set_drvdata(pdev, port);
+
+        return uart_add_one_port(&msm_uart_driver, port);
+}
+
+static int __devexit msm_serial_remove(struct platform_device *pdev)
+{
+        struct msm_port *msm_port = platform_get_drvdata(pdev);
+
+        clk_put(msm_port->clk);
+
+        return 0;
+}
+
+static struct platform_driver msm_platform_driver = {
+        .probe = msm_serial_probe,
+        .remove = msm_serial_remove,
+        .driver = {
+                .name = "msm_serial",
+                .owner = THIS_MODULE,
+        },
+};
+
+static int __init msm_serial_init(void)
+{
+        int ret;
+
+        ret = uart_register_driver(&msm_uart_driver);
+        if (unlikely(ret))
+                return ret;
+
+        ret = platform_driver_probe(&msm_platform_driver, msm_serial_probe);
+        if (unlikely(ret))
+                uart_unregister_driver(&msm_uart_driver);
+
+        printk(KERN_INFO "msm_serial: driver initialized\n");
+
+        return ret;
+}
+
+static void __exit msm_serial_exit(void)
+{
+#ifdef CONFIG_SERIAL_MSM_CONSOLE
+        unregister_console(&msm_console);
+#endif
+        platform_driver_unregister(&msm_platform_driver);
+        uart_unregister_driver(&msm_uart_driver);
+}
+
+module_init(msm_serial_init);
+module_exit(msm_serial_exit);
+
+MODULE_AUTHOR("Robert Love <rlove@google.com>");
+MODULE_DESCRIPTION("Driver for msm7x serial device");
+MODULE_LICENSE("GPL");
diff --git a/drivers/serial/msm_serial.h b/drivers/serial/msm_serial.h
new file mode 100644
index 000000000000..689f1fa0e84e
--- /dev/null
+++ b/drivers/serial/msm_serial.h
@@ -0,0 +1,117 @@
+/*
+ * drivers/serial/msm_serial.h
+ *
+ * Copyright (C) 2007 Google, Inc.
+ * Author: Robert Love <rlove@google.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __DRIVERS_SERIAL_MSM_SERIAL_H
+#define __DRIVERS_SERIAL_MSM_SERIAL_H
+
+#define UART_MR1                        0x0000
+
+#define UART_MR1_AUTO_RFR_LEVEL0        0x3F
+#define UART_MR1_AUTO_RFR_LEVEL1        0x3FF00
+#define UART_MR1_RX_RDY_CTL             (1 << 7)
+#define UART_MR1_CTS_CTL                (1 << 6)
+
+#define UART_MR2                        0x0004
+#define UART_MR2_ERROR_MODE             (1 << 6)
+#define UART_MR2_BITS_PER_CHAR          0x30
+#define UART_MR2_BITS_PER_CHAR_5        (0x0 << 4)
+#define UART_MR2_BITS_PER_CHAR_6        (0x1 << 4)
+#define UART_MR2_BITS_PER_CHAR_7        (0x2 << 4)
+#define UART_MR2_BITS_PER_CHAR_8        (0x3 << 4)
+#define UART_MR2_STOP_BIT_LEN_ONE       (0x1 << 2)
+#define UART_MR2_STOP_BIT_LEN_TWO       (0x3 << 2)
+#define UART_MR2_PARITY_MODE_NONE       0x0
+#define UART_MR2_PARITY_MODE_ODD        0x1
+#define UART_MR2_PARITY_MODE_EVEN       0x2
+#define UART_MR2_PARITY_MODE_SPACE      0x3
+#define UART_MR2_PARITY_MODE            0x3
+
+#define UART_CSR        0x0008
+#define UART_CSR_115200 0xFF
+#define UART_CSR_57600  0xEE
+#define UART_CSR_38400  0xDD
+#define UART_CSR_28800  0xCC
+#define UART_CSR_19200  0xBB
+#define UART_CSR_14400  0xAA
+#define UART_CSR_9600   0x99
+#define UART_CSR_4800   0x77
+#define UART_CSR_2400   0x55
+#define UART_CSR_1200   0x44
+#define UART_CSR_600    0x33
+#define UART_CSR_300    0x22
+
+#define UART_TF         0x000C
+
+#define UART_CR                         0x0010
+#define UART_CR_CMD_NULL                (0 << 4)
+#define UART_CR_CMD_RESET_RX            (1 << 4)
+#define UART_CR_CMD_RESET_TX            (2 << 4)
+#define UART_CR_CMD_RESET_ERR           (3 << 4)
+#define UART_CR_CMD_RESET_BREAK_INT     (4 << 4)
+#define UART_CR_CMD_START_BREAK         (5 << 4)
+#define UART_CR_CMD_STOP_BREAK          (6 << 4)
+#define UART_CR_CMD_RESET_CTS           (7 << 4)
+#define UART_CR_CMD_PACKET_MODE         (9 << 4)
+#define UART_CR_CMD_MODE_RESET          (12 << 4)
+#define UART_CR_CMD_SET_RFR             (13 << 4)
+#define UART_CR_CMD_RESET_RFR           (14 << 4)
+#define UART_CR_TX_DISABLE              (1 << 3)
+#define UART_CR_TX_ENABLE               (1 << 3)
+#define UART_CR_RX_DISABLE              (1 << 3)
+#define UART_CR_RX_ENABLE               (1 << 3)
+
+#define UART_IMR                0x0014
+#define UART_IMR_TXLEV          (1 << 0)
+#define UART_IMR_RXSTALE        (1 << 3)
+#define UART_IMR_RXLEV          (1 << 4)
+#define UART_IMR_DELTA_CTS      (1 << 5)
+#define UART_IMR_CURRENT_CTS    (1 << 6)
+
+#define UART_IPR_RXSTALE_LAST           0x20
+#define UART_IPR_STALE_LSB              0x1F
+#define UART_IPR_STALE_TIMEOUT_MSB      0x3FF80
+
+#define UART_IPR        0x0018
+#define UART_TFWR       0x001C
+#define UART_RFWR       0x0020
+#define UART_HCR        0x0024
+
+#define UART_MREG               0x0028
+#define UART_NREG               0x002C
+#define UART_DREG               0x0030
+#define UART_MNDREG             0x0034
+#define UART_IRDA               0x0038
+#define UART_MISR_MODE          0x0040
+#define UART_MISR_RESET         0x0044
+#define UART_MISR_EXPORT        0x0048
+#define UART_MISR_VAL           0x004C
+#define UART_TEST_CTRL          0x0050
+
+#define UART_SR                 0x0008
+#define UART_SR_HUNT_CHAR       (1 << 7)
+#define UART_SR_RX_BREAK        (1 << 6)
+#define UART_SR_PAR_FRAME_ERR   (1 << 5)
+#define UART_SR_OVERRUN         (1 << 4)
+#define UART_SR_TX_EMPTY        (1 << 3)
+#define UART_SR_TX_READY        (1 << 2)
+#define UART_SR_RX_FULL         (1 << 1)
+#define UART_SR_RX_READY        (1 << 0)
+
+#define UART_RF         0x000C
+#define UART_MISR       0x0010
+#define UART_ISR        0x0014
+
+#endif  /* __DRIVERS_SERIAL_MSM_SERIAL_H */
diff --git a/drivers/serial/s3c2400.c b/drivers/serial/s3c2400.c
index 4873f2978bd2..fb00ed5296e6 100644
--- a/drivers/serial/s3c2400.c
+++ b/drivers/serial/s3c2400.c
@@ -78,7 +78,7 @@ static int s3c2400_serial_probe(struct platform_device *dev)
 
 static struct platform_driver s3c2400_serial_drv = {
         .probe          = s3c2400_serial_probe,
-        .remove         = s3c24xx_serial_remove,
+        .remove         = __devexit_p(s3c24xx_serial_remove),
         .driver         = {
                 .name   = "s3c2400-uart",
                 .owner  = THIS_MODULE,
diff --git a/drivers/serial/s3c2410.c b/drivers/serial/s3c2410.c
index 87c182ef71b8..b5d7cbcba2ae 100644
--- a/drivers/serial/s3c2410.c
+++ b/drivers/serial/s3c2410.c
@@ -90,7 +90,7 @@ static int s3c2410_serial_probe(struct platform_device *dev)
 
 static struct platform_driver s3c2410_serial_drv = {
         .probe          = s3c2410_serial_probe,
-        .remove         = s3c24xx_serial_remove,
+        .remove         = __devexit_p(s3c24xx_serial_remove),
         .driver         = {
                 .name   = "s3c2410-uart",
                 .owner  = THIS_MODULE,
diff --git a/drivers/serial/s3c2412.c b/drivers/serial/s3c2412.c
index fd017b375568..11dcb90bdfef 100644
--- a/drivers/serial/s3c2412.c
+++ b/drivers/serial/s3c2412.c
@@ -123,7 +123,7 @@ static int s3c2412_serial_probe(struct platform_device *dev)
 
 static struct platform_driver s3c2412_serial_drv = {
         .probe          = s3c2412_serial_probe,
-        .remove         = s3c24xx_serial_remove,
+        .remove         = __devexit_p(s3c24xx_serial_remove),
         .driver         = {
                 .name   = "s3c2412-uart",
                 .owner  = THIS_MODULE,
diff --git a/drivers/serial/s3c2440.c b/drivers/serial/s3c2440.c
index 29cbb0afef8e..06c5b0cc47a3 100644
--- a/drivers/serial/s3c2440.c
+++ b/drivers/serial/s3c2440.c
@@ -153,7 +153,7 @@ static int s3c2440_serial_probe(struct platform_device *dev)
 
 static struct platform_driver s3c2440_serial_drv = {
         .probe          = s3c2440_serial_probe,
-        .remove         = s3c24xx_serial_remove,
+        .remove         = __devexit_p(s3c24xx_serial_remove),
         .driver         = {
                 .name   = "s3c2440-uart",
                 .owner  = THIS_MODULE,
diff --git a/drivers/serial/s3c24a0.c b/drivers/serial/s3c24a0.c
index ebf2fd3c8f7d..786a067d62ac 100644
--- a/drivers/serial/s3c24a0.c
+++ b/drivers/serial/s3c24a0.c
@@ -94,7 +94,7 @@ static int s3c24a0_serial_probe(struct platform_device *dev)
 
 static struct platform_driver s3c24a0_serial_drv = {
         .probe          = s3c24a0_serial_probe,
-        .remove         = s3c24xx_serial_remove,
+        .remove         = __devexit_p(s3c24xx_serial_remove),
         .driver         = {
                 .name   = "s3c24a0-uart",
                 .owner  = THIS_MODULE,
diff --git a/drivers/serial/s3c6400.c b/drivers/serial/s3c6400.c
index 3e3785233682..48f1a3781f0d 100644
--- a/drivers/serial/s3c6400.c
+++ b/drivers/serial/s3c6400.c
@@ -124,7 +124,7 @@ static int s3c6400_serial_probe(struct platform_device *dev)
 
 static struct platform_driver s3c6400_serial_drv = {
         .probe          = s3c6400_serial_probe,
-        .remove         = s3c24xx_serial_remove,
+        .remove         = __devexit_p(s3c24xx_serial_remove),
         .driver         = {
                 .name   = "s3c6400-uart",
                 .owner  = THIS_MODULE,
diff --git a/drivers/serial/samsung.c b/drivers/serial/samsung.c
index 93b5d75db126..c8851a0db63a 100644
--- a/drivers/serial/samsung.c
+++ b/drivers/serial/samsung.c
@@ -1174,7 +1174,7 @@ int s3c24xx_serial_probe(struct platform_device *dev,
 
 EXPORT_SYMBOL_GPL(s3c24xx_serial_probe);
 
-int s3c24xx_serial_remove(struct platform_device *dev)
+int __devexit s3c24xx_serial_remove(struct platform_device *dev)
 {
         struct uart_port *port = s3c24xx_dev_to_port(&dev->dev);
 
diff --git a/drivers/serial/samsung.h b/drivers/serial/samsung.h
index 7afb94843a08..d3fe315969f6 100644
--- a/drivers/serial/samsung.h
+++ b/drivers/serial/samsung.h
@@ -72,7 +72,7 @@ struct s3c24xx_uart_port {
 extern int s3c24xx_serial_probe(struct platform_device *dev,
                                 struct s3c24xx_uart_info *uart);
 
-extern int s3c24xx_serial_remove(struct platform_device *dev);
+extern int __devexit s3c24xx_serial_remove(struct platform_device *dev);
 
 extern int s3c24xx_serial_initconsole(struct platform_driver *drv,
                                       struct s3c24xx_uart_info *uart);
diff --git a/drivers/serial/sb1250-duart.c b/drivers/serial/sb1250-duart.c
index a4fb343a08da..319e8b83f6be 100644
--- a/drivers/serial/sb1250-duart.c
+++ b/drivers/serial/sb1250-duart.c
@@ -204,7 +204,7 @@ static int sbd_receive_drain(struct sbd_port *sport)
 {
         int loops = 10000;
 
-        while (sbd_receive_ready(sport) && loops--)
+        while (sbd_receive_ready(sport) && --loops)
                 read_sbdchn(sport, R_DUART_RX_HOLD);
         return loops;
 }
@@ -218,7 +218,7 @@ static int __maybe_unused sbd_transmit_drain(struct sbd_port *sport)
 {
         int loops = 10000;
 
-        while (!sbd_transmit_ready(sport) && loops--)
+        while (!sbd_transmit_ready(sport) && --loops)
                 udelay(2);
         return loops;
 }
@@ -232,7 +232,7 @@ static int sbd_line_drain(struct sbd_port *sport)
 {
         int loops = 10000;
 
-        while (!sbd_transmit_empty(sport) && loops--)
+        while (!sbd_transmit_empty(sport) && --loops)
                 udelay(2);
         return loops;
 }
diff --git a/drivers/serial/sunhv.c b/drivers/serial/sunhv.c
index a94a2ab4b571..1df5325faab2 100644
--- a/drivers/serial/sunhv.c
+++ b/drivers/serial/sunhv.c
@@ -461,7 +461,7 @@ static void sunhv_console_write_paged(struct console *con, const char *s, unsign
                                         break;
                                 udelay(1);
                         }
-                        if (limit <= 0)
+                        if (limit < 0)
                                 break;
                         page_bytes -= written;
                         ra += written;
diff --git a/drivers/serial/timbuart.c b/drivers/serial/timbuart.c
index ac9e5d5f742e..063a313b755c 100644
--- a/drivers/serial/timbuart.c
+++ b/drivers/serial/timbuart.c
@@ -33,29 +33,29 @@ struct timbuart_port {
         struct uart_port        port;
         struct tasklet_struct   tasklet;
         int                     usedma;
-        u8                      last_ier;
+        u32                     last_ier;
         struct platform_device  *dev;
 };
 
 static int baudrates[] = {9600, 19200, 38400, 57600, 115200, 230400, 460800,
         921600, 1843200, 3250000};
 
-static void timbuart_mctrl_check(struct uart_port *port, u8 isr, u8 *ier);
+static void timbuart_mctrl_check(struct uart_port *port, u32 isr, u32 *ier);
 
 static irqreturn_t timbuart_handleinterrupt(int irq, void *devid);
 
 static void timbuart_stop_rx(struct uart_port *port)
 {
         /* spin lock held by upper layer, disable all RX interrupts */
-        u8 ier = ioread8(port->membase + TIMBUART_IER) & ~RXFLAGS;
-        iowrite8(ier, port->membase + TIMBUART_IER);
+        u32 ier = ioread32(port->membase + TIMBUART_IER) & ~RXFLAGS;
+        iowrite32(ier, port->membase + TIMBUART_IER);
 }
 
 static void timbuart_stop_tx(struct uart_port *port)
 {
         /* spinlock held by upper layer, disable TX interrupt */
-        u8 ier = ioread8(port->membase + TIMBUART_IER) & ~TXBAE;
-        iowrite8(ier, port->membase + TIMBUART_IER);
+        u32 ier = ioread32(port->membase + TIMBUART_IER) & ~TXBAE;
+        iowrite32(ier, port->membase + TIMBUART_IER);
 }
 
 static void timbuart_start_tx(struct uart_port *port)
@@ -72,14 +72,14 @@ static void timbuart_flush_buffer(struct uart_port *port)
         u8 ctl = ioread8(port->membase + TIMBUART_CTRL) | TIMBUART_CTRL_FLSHTX;
 
         iowrite8(ctl, port->membase + TIMBUART_CTRL);
-        iowrite8(TXBF, port->membase + TIMBUART_ISR);
+        iowrite32(TXBF, port->membase + TIMBUART_ISR);
 }
 
 static void timbuart_rx_chars(struct uart_port *port)
 {
         struct tty_struct *tty = port->info->port.tty;
 
-        while (ioread8(port->membase + TIMBUART_ISR) & RXDP) {
+        while (ioread32(port->membase + TIMBUART_ISR) & RXDP) {
                 u8 ch = ioread8(port->membase + TIMBUART_RXFIFO);
                 port->icount.rx++;
                 tty_insert_flip_char(tty, ch, TTY_NORMAL);
@@ -97,7 +97,7 @@ static void timbuart_tx_chars(struct uart_port *port)
 {
         struct circ_buf *xmit = &port->info->xmit;
 
-        while (!(ioread8(port->membase + TIMBUART_ISR) & TXBF) &&
+        while (!(ioread32(port->membase + TIMBUART_ISR) & TXBF) &&
                 !uart_circ_empty(xmit)) {
                 iowrite8(xmit->buf[xmit->tail],
                         port->membase + TIMBUART_TXFIFO);
@@ -114,7 +114,7 @@ static void timbuart_tx_chars(struct uart_port *port)
                 ioread8(port->membase + TIMBUART_BAUDRATE));
 }
 
-static void timbuart_handle_tx_port(struct uart_port *port, u8 isr, u8 *ier)
+static void timbuart_handle_tx_port(struct uart_port *port, u32 isr, u32 *ier)
 {
         struct timbuart_port *uart =
                 container_of(port, struct timbuart_port, port);
@@ -129,7 +129,7 @@ static void timbuart_handle_tx_port(struct uart_port *port, u8 isr, u8 *ier)
         if (isr & TXFLAGS) {
                 timbuart_tx_chars(port);
                 /* clear all TX interrupts */
-                iowrite8(TXFLAGS, port->membase + TIMBUART_ISR);
+                iowrite32(TXFLAGS, port->membase + TIMBUART_ISR);
 
                 if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
                         uart_write_wakeup(port);
@@ -148,7 +148,7 @@ static void timbuart_handle_tx_port(struct uart_port *port, u8 isr, u8 *ier)
         dev_dbg(port->dev, "%s - leaving\n", __func__);
 }
 
-void timbuart_handle_rx_port(struct uart_port *port, u8 isr, u8 *ier)
+void timbuart_handle_rx_port(struct uart_port *port, u32 isr, u32 *ier)
 {
         if (isr & RXFLAGS) {
                 /* Some RX status is set */
@@ -161,7 +161,7 @@ void timbuart_handle_rx_port(struct uart_port *port, u8 isr, u8 *ier)
                         timbuart_rx_chars(port);
 
                 /* ack all RX interrupts */
-                iowrite8(RXFLAGS, port->membase + TIMBUART_ISR);
+                iowrite32(RXFLAGS, port->membase + TIMBUART_ISR);
         }
 
         /* always have the RX interrupts enabled */
@@ -173,11 +173,11 @@ void timbuart_handle_rx_port(struct uart_port *port, u8 isr, u8 *ier)
 void timbuart_tasklet(unsigned long arg)
 {
         struct timbuart_port *uart = (struct timbuart_port *)arg;
-        u8 isr, ier = 0;
+        u32 isr, ier = 0;
 
         spin_lock(&uart->port.lock);
 
-        isr = ioread8(uart->port.membase + TIMBUART_ISR);
+        isr = ioread32(uart->port.membase + TIMBUART_ISR);
         dev_dbg(uart->port.dev, "%s ISR: %x\n", __func__, isr);
 
         if (!uart->usedma)
@@ -188,7 +188,7 @@ void timbuart_tasklet(unsigned long arg)
         if (!uart->usedma)
                 timbuart_handle_rx_port(&uart->port, isr, &ier);
 
-        iowrite8(ier, uart->port.membase + TIMBUART_IER);
+        iowrite32(ier, uart->port.membase + TIMBUART_IER);
 
         spin_unlock(&uart->port.lock);
         dev_dbg(uart->port.dev, "%s leaving\n", __func__);
@@ -196,9 +196,9 @@ void timbuart_tasklet(unsigned long arg)
 
 static unsigned int timbuart_tx_empty(struct uart_port *port)
 {
-        u8 isr = ioread8(port->membase + TIMBUART_ISR);
+        u32 isr = ioread32(port->membase + TIMBUART_ISR);
 
-        return (isr & TXBAE) ? TIOCSER_TEMT : 0;
+        return (isr & TXBE) ? TIOCSER_TEMT : 0;
 }
 
 static unsigned int timbuart_get_mctrl(struct uart_port *port)
@@ -222,13 +222,13 @@ static void timbuart_set_mctrl(struct uart_port *port, unsigned int mctrl)
                 iowrite8(TIMBUART_CTRL_RTS, port->membase + TIMBUART_CTRL);
 }
 
-static void timbuart_mctrl_check(struct uart_port *port, u8 isr, u8 *ier)
+static void timbuart_mctrl_check(struct uart_port *port, u32 isr, u32 *ier)
 {
         unsigned int cts;
 
         if (isr & CTS_DELTA) {
                 /* ack */
-                iowrite8(CTS_DELTA, port->membase + TIMBUART_ISR);
+                iowrite32(CTS_DELTA, port->membase + TIMBUART_ISR);
                 cts = timbuart_get_mctrl(port);
                 uart_handle_cts_change(port, cts & TIOCM_CTS);
                 wake_up_interruptible(&port->info->delta_msr_wait);
@@ -255,9 +255,9 @@ static int timbuart_startup(struct uart_port *port)
         dev_dbg(port->dev, "%s\n", __func__);
 
         iowrite8(TIMBUART_CTRL_FLSHRX, port->membase + TIMBUART_CTRL);
-        iowrite8(0xff, port->membase + TIMBUART_ISR);
+        iowrite32(0x1ff, port->membase + TIMBUART_ISR);
         /* Enable all but TX interrupts */
-        iowrite8(RXBAF | RXBF | RXTT | CTS_DELTA,
+        iowrite32(RXBAF | RXBF | RXTT | CTS_DELTA,
                 port->membase + TIMBUART_IER);
 
         return request_irq(port->irq, timbuart_handleinterrupt, IRQF_SHARED,
@@ -270,7 +270,7 @@ static void timbuart_shutdown(struct uart_port *port)
                 container_of(port, struct timbuart_port, port);
         dev_dbg(port->dev, "%s\n", __func__);
         free_irq(port->irq, uart);
-        iowrite8(0, port->membase + TIMBUART_IER);
+        iowrite32(0, port->membase + TIMBUART_IER);
 }
 
 static int get_bindex(int baud)
@@ -359,10 +359,10 @@ static irqreturn_t timbuart_handleinterrupt(int irq, void *devid)
         struct timbuart_port *uart = (struct timbuart_port *)devid;
 
         if (ioread8(uart->port.membase + TIMBUART_IPR)) {
-                uart->last_ier = ioread8(uart->port.membase + TIMBUART_IER);
+                uart->last_ier = ioread32(uart->port.membase + TIMBUART_IER);
 
                 /* disable interrupts, the tasklet enables them again */
-                iowrite8(0, uart->port.membase + TIMBUART_IER);
+                iowrite32(0, uart->port.membase + TIMBUART_IER);
 
                 /* fire off bottom half */
                 tasklet_schedule(&uart->tasklet);
diff --git a/drivers/serial/zs.c b/drivers/serial/zs.c
index 9e6a873f8203..d8c2809b1ab6 100644
--- a/drivers/serial/zs.c
+++ b/drivers/serial/zs.c
@@ -231,7 +231,7 @@ static int zs_receive_drain(struct zs_port *zport)
 {
         int loops = 10000;
 
-        while ((read_zsreg(zport, R0) & Rx_CH_AV) && loops--)
+        while ((read_zsreg(zport, R0) & Rx_CH_AV) && --loops)
                 read_zsdata(zport);
         return loops;
 }
@@ -241,7 +241,7 @@ static int zs_transmit_drain(struct zs_port *zport, int irq)
         struct zs_scc *scc = zport->scc;
         int loops = 10000;
 
-        while (!(read_zsreg(zport, R0) & Tx_BUF_EMP) && loops--) {
+        while (!(read_zsreg(zport, R0) & Tx_BUF_EMP) && --loops) {
                 zs_spin_unlock_cond_irq(&scc->zlock, irq);
                 udelay(2);
                 zs_spin_lock_cond_irq(&scc->zlock, irq);
@@ -254,7 +254,7 @@ static int zs_line_drain(struct zs_port *zport, int irq)
         struct zs_scc *scc = zport->scc;
         int loops = 10000;
 
-        while (!(read_zsreg(zport, R1) & ALL_SNT) && loops--) {
+        while (!(read_zsreg(zport, R1) & ALL_SNT) && --loops) {
                 zs_spin_unlock_cond_irq(&scc->zlock, irq);
                 udelay(2);
                 zs_spin_lock_cond_irq(&scc->zlock, irq);
diff --git a/drivers/staging/serqt_usb2/serqt_usb2.c b/drivers/staging/serqt_usb2/serqt_usb2.c
index 581232b719fd..90b29b564631 100644
--- a/drivers/staging/serqt_usb2/serqt_usb2.c
+++ b/drivers/staging/serqt_usb2/serqt_usb2.c
@@ -284,21 +284,12 @@ static void ProcessModemStatus(struct quatech_port *qt_port,
         return;
 }
 
-static void ProcessRxChar(struct usb_serial_port *port, unsigned char Data)
+static void ProcessRxChar(struct tty_struct *tty, struct usb_serial_port *port,
+                                                unsigned char data)
 {
-        struct tty_struct *tty;
         struct urb *urb = port->read_urb;
-        tty = tty_port_tty_get(&port->port);
-
-        /* if we insert more than TTY_FLIPBUF_SIZE characters, we drop them. */
-
-        if (tty && urb->actual_length) {
-                tty_buffer_request_room(tty, 1);
-                tty_insert_flip_string(tty, &Data, 1);
-                /* tty_flip_buffer_push(tty); */
-        }
-
-        return;
+        if (urb->actual_length)
+                tty_insert_flip_char(tty, data, TTY_NORMAL);
 }
 
 static void qt_write_bulk_callback(struct urb *urb)
@@ -435,8 +426,10 @@ static void qt_read_bulk_callback(struct urb *urb)
                                 case 0xff:
                                         dbg("No status sequence. \n");
 
-                                        ProcessRxChar(port, data[i]);
-                                        ProcessRxChar(port, data[i + 1]);
+                                        if (tty) {
+                                                ProcessRxChar(tty, port, data[i]);
+                                                ProcessRxChar(tty, port, data[i + 1]);
+                                        }
                                         i += 2;
                                         break;
                                 }
@@ -444,10 +437,8 @@ static void qt_read_bulk_callback(struct urb *urb)
                                         continue;
                         }
 
-                        if (tty && urb->actual_length) {
-                                tty_buffer_request_room(tty, 1);
-                                tty_insert_flip_string(tty, (data + i), 1);
-                        }
+                        if (tty && urb->actual_length)
+                                tty_insert_flip_char(tty, data[i], TTY_NORMAL);
 
                 }
                 tty_flip_buffer_push(tty);